A Human-Following Motion Planning and Control Scheme for Collaborative Robots Based on Human Motion Prediction

Abstract

Human–Robot Interaction (HRI) for collaborative robots has become an active research topic recently. Collaborative robots assist human workers in their tasks and improve their efficiency. However, the worker should also feel safe and comfortable while interacting with the robot. In this paper, we propose a human-following motion planning and control scheme for a collaborative robot which supplies the necessary parts and tools to a worker in an assembly process in a factory. In our proposed scheme, a 3-D sensing system is employed to measure the skeletal data of the worker. At each sampling time of the sensing system, an optimal delivery position is estimated using the real-time worker data. At the same time, the future positions of the worker are predicted as probabilistic distributions. A Model Predictive Control (MPC)-based trajectory planner is used to calculate a robot trajectory that supplies the required parts and tools to the worker and follows the predicted future positions of the worker. We have installed our proposed scheme in a collaborative robot system with a 2-DOF planar manipulator. Experimental results show that the proposed scheme enables the robot to provide anytime assistance to a worker who is moving around in the workspace while ensuring the safety and comfort of the worker.

1. Introduction

The concept of collaborative robots was introduced in the early 1990s. The first collaborative system was proposed by Troccaz et al. in 1993 [1]. This system uses a passive robot arm to ensure safe operation during medical procedures. In 1996, Colgate et al. developed a passive collaborative robot system and applied it to the vehicle’s door assembly process carried out by a human worker [2]. In 1999, Yamada et al. proposed a skill-assist system to help a human worker carry a heavy load [3].

The collaborative robot systems are being actively introduced in the manufacturing industry. The International Organization of Standardization (ISO) amended its robot safety standards ISO 10128-1 [4] and 10128-2 [5] in 2011 to include the safety guidelines for human-robot collaboration. This led to an exponential rise in collaborative robot research and development. Today, many companies are manufacturing their own versions of collaborative robots, and these robots are being used in industries all over the world. Collaborative robots are expected to play a major role in the Industry 5.0 environments where people will work together with robots and smart machines [6].

In 2010, a 2-DOF co-worker robot “PaDY” (in-time Parts and tools Delivery to You robot) was developed in our lab to assist a factory worker in an automobile assembly process [7]. This process comprises a set of assembly tasks that are carried out by a worker while moving around the car body. PaDY assists the worker by delivering the necessary parts and tools to him/her for each task. The original control system of PaDY was developed based on a statistical analysis of the worker’s movements [7].

Many studies have been carried out on the human–robot collaborative system. Hawkins et al. proposed an inference mechanism of human action based on a probabilistic model to achieve wait-sensitive robot motion planning in 2013 [8]. D’Arpino et al. proposed fast target prediction of human reaching motion for human–robot collaborative tasks in 2015 [9]. Unhelkar et al. designed a human-aware robotic system, in which human motion prediction is used to achieve a safe and efficient part-delivery task between the robot and the stationary human worker in 2018 [10]. A recent survey on the sensors and techniques used for human detection and action recognition in industrial environments can be seen in [11]. The studies cited above [8,9,10] have improved efficiency of the collaborative tasks by incorporating human motion prediction into robot motion planning.

Human motion prediction was also introduced to PaDY. In 2012, the delivery operation delay of the robot–human handover tasks was reduced by utilizing prediction of the worker’s arrival time at the predetermined working position [12]. In 2019, a motion planning system was developed which optimized a robot trajectory by taking the prediction uncertainty of the worker’s movement into account [13]. In those studies [12,13], the robot repeats the delivery motion from its home position to each predetermined assembly position. If the robot can follow the worker during the assembly process, the worker can pick up necessary parts and tools from the robot at any time. Thus, more efficient collaborative work could be expected by introducing human-following motion.

In this paper, human-following motion of the collaborative robot is proposed for delivery of parts and tools to the worker. The human-following collaborative robot system needs to stay close to the human worker, while avoiding collision with the worker under the velocity and acceleration constraints. The contribution of this paper is summarized as follows:

1

The proposed human-following motion planning and control scheme enables the worker to pick up the necessary parts and tools when needed.

2

The proposed scheme achieves the human-following motion with a sufficiently small tracking error without adversely affecting the safety and comfort of the worker.

3

Experiments conducted in an environment similar to a real automobile assembly process illustrate the effectiveness of the proposed scheme.

This is a quantitative study where we conducted the experiments ourselves and analyzed the data collected from these experiments to deduce the results. The proposed scheme predicts the motion of the worker and calculates an optimal delivery position for the handover of parts and tools from the worker to the robot for each task of the assembly process. This scheme has been designed for a single worker operating within his/her workspace. It is not designed for the cases when multiple workers are operating in the same workspace, or when the worker moves beyond the workspace.

The rest of the paper is organized as follows. Section 2 describes the related works. Section 3 gives an overview of the proposed scheme, including the delivery position determination, the worker’s motion prediction, and trajectory planning and control scheme. The experimental results are discussed in Section 4. Section 5 concludes this paper.

2. Related Works

In this section, we present a review of the existing research on human–robot handover tasks, human-following robots, and motion/task planning based on human motion prediction.

2.1. Human–Robot Handover

Some studies have considered the problem of psychological comfort of the human receiver during the handover task. Baraglia et al. addressed the issue of whether and when a robot should take the initiative [14]. Cakmak et al. advocated the inclusion of user preferences while calculating handover position [15]. They also identified that a major cause of delay in the handover action is the failure to convey the intention and timing of handing over the object from the robot to the human [16]. Although these studies deal with important issues for improving the human–robot collaboration, it is still difficult to apply them in actual applications because psychological factors cannot be directly observed.

Some other studies used observable physical characteristics of the human worker for planning a robot motion that is safe and comfortable for the worker. Mainprice et al. proposed a motion planning scheme for human–robot collaboration considering HRI constraints such as constraints of distance, visibility and arm comfort of the worker [17]. Aleotti et al. devised a scheme in which the object is delivered in such a way that its most easily graspable part is directed towards the worker [18]. Sisbot et al. proposed a human-aware motion planner that is safe, comfortable and socially acceptable for the human worker [19].

The techniques and algorithms mentioned above operate with the assumption that the worker remains stationary in the environment. To solve the problem of providing assistance to a worker who moves around in the environment, we propose a human-following approach with HRI constraints in this paper.

2.2. Human-Following Robots

Several techniques have been proposed to carry out human-following motion in various robot applications. One of the first human-following approaches was proposed by Nagumo et al., in which an LED device carried by the human was detected and tracked by the robot using a camera [20].

Hirai et al. performed visual tracking of the human back and shoulder in order to follow a person [21]. Yoshimi et al. used several parameters including the distance, speed, color and texture of human clothes to achieve stable tracking in complex situations [22]. Morioka et al. used the reference velocities for human-following control calculated from estimated human position under the uncertainty of the recognition [23]. Suda et al. proposed a human–robot cooperative handling control using force and moment information [24].

The techniques cited in this section focus on performing human-following motion of the robot to achieve safe and continuous tracking. However, these schemes use the feedback of the observed/estimated current position of the worker. This makes it difficult for the robot to keep up with the worker who is continuously moving around in the workspace. In this paper, we solve this problem by applying human motion prediction and MPC.

2.3. Motion/Task Planning Based on Human Motion Prediction

In recent years, many studies have proposed motion planning using human motion prediction. The predicted human motion is used to generate a safe robot trajectory. Mainprice et al. proposed to plan a motion that avoids the predicted occupancy of the 3D human body [25]. Fridovich-Keil et al. proposed to plan a motion that avoids the risky region calculated by the confidence-aware human motion prediction [26]. Park et al. proposed a collision-free motion planner using the probabilistic collision checker [27].

Several studies have proposed robot task planning to achieve collaborative work based on human motion prediction. Maeda et al. achieved a fluid human–robot handover by estimating the phase of human motion [28]. Liu et al. presented a probabilistic model for human motion prediction for task-level human–robot collaborative assembly [29]. Cheng et al. proposed an integrated framework for human–robot collaboration in which the robot perceives and adapts to human actions [30].

Human motion prediction has been effectively used in various problems of human–robot interaction. In this paper, we apply the human motion prediction to human-following motion of the collaborative robot for delivery of parts/tools to a worker.

3. Proposed Motion Planning and Control Scheme

3.1. System Architecture

Figure 1 shows the system architecture of our proposed scheme. This scheme consists of three major parts:

1

Delivery position determination;

2

Worker’s motion prediction;

3

Trajectory planning and control.

In delivery position determination, an optimal delivery position is estimated using an HRI-based cost function. This cost function is calculated using the skeletal data of the worker measured by the 3-D vision sensor. In workers’ motion prediction, the position data obtained from the vision sensor are used to predict the motion of the worker. Moreover, after the completion of each work cycle, the worker’s model is updated using the stored position data. In the trajectory planning step, an optimal trajectory from the robot’s current position to the goal position is calculated using the receding horizon principle of Model Predictive Control (MPC). The robot motion controller ensures that the robot follows the calculated trajectory. The detailed description of these three parts of our scheme is given in the subsequent subsections.

3.2. Delivery Position Determination

In our proposed scheme, the delivery position is determined by optimizing an HRI-based cost function. This cost function includes terms related to the safety, visibility and arm comfort of the worker. These terms are calculated from the worker’s skeletal data observed by the 3-D vision sensor in real-time. This concept was first introduced by Sisbot et al. for motion planning of mobile robots [31]. The analytical form of the HRI-based cost function was proposed in our previous study [32]. Here, we provide a brief description of the cost function and solver for determining the optimal delivery position.

Let ${\mathit{p}}_{del}\in {\mathbb{R}}^n$ be the n dimensional delivery position, then the total cost $Cost({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})$ is expressed as:

$$\begin{array}{c}\hfill Cost({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})=C_V({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})+C_S({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})+C_A({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})\end{array}$$

where ${\mathit{s}}_{\mathrm{w}}$ is latest sample of the worker’s skeletal data obtained from the sensor. $C_V({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})$ is the visibility cost that maintains the delivery position within the visual range of the worker. This cost is expressed as a function of the difference between the worker’s body orientation and the direction of the delivery position with respect to the worker’s body center. $C_S({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})$ is the safety cost that prevents the robot from colliding with the worker. This cost is expressed as a function of the distance between the worker’s body center and the delivery position. $C_A({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})$ is the arm comfort cost that maintains the delivery position within the suitable distance and orientation for the worker. This cost is a function of the joint angles of the worker’s arm. In addition, this cost penalizes the delivery position where the worker needs to use his/her non-dominant hand.

The optimal delivery position is calculated by minimizing the cost function $Cost({\mathit{p}}_{del}$$,{\mathit{s}}_{\mathrm{w}})$. Since $Cost({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})$ is a non-convex function, we use Transition-based Rapidly-exploring Random Tree(T-RRT) method [33] to find the globally optimal solution. We apply T-RRT only in the vicinity of the worker to calculate the optimal solution in real-time. The process of determining the optimal delivery position is summarized in Algorithm 1.

Algorithm 1: Determination of Optimal Delivery Position using T-RRT

Input: Worker’s position ${\mathit{p}}_{\mathrm{w}}$,

Current sample of the worker’s skeleton ${\mathit{s}}_{\mathrm{w}}$,

Sampling range $r_{\mathrm{s}}$,

HRI cost function $Cost({\mathit{p}}_{del},{\mathit{s}}_{\mathrm{w}})$

Output: Optimal delivery position ${\mathit{p}}_{del}$

1: Set the sampling area $S_{\mathrm{near}}$ using ${\mathit{p}}_{\mathrm{w}}$ and $r_{\mathrm{s}}$

2: ${\mathit{p}}_{cur}\leftarrow Sample\left(S_{near}\right)$

3: $Cos{t}_{cur}\leftarrow Cost({\mathit{p}}_{cur},{\mathit{s}}_{\mathrm{w}})$

4: $Counter\leftarrow 0$

5: while$Counter\le Counte{r}_{max}$do

6:  ${\mathit{p}}_{rand}\leftarrow Sample\left(S_{near}\right)$

7:  ${\mathit{p}}_{new}\leftarrow {\mathit{p}}_{cur}+\delta ({\mathit{p}}_{rand}-{\mathit{p}}_{cur})$

8:  $Cos{t}_{new}\leftarrow Cost({\mathit{p}}_{new},{\mathit{s}}_{\mathrm{w}})$

9:  if $TransitionTest(Cos{t}_{new},Cos{t}_{cur},d_{new-cur})$ then

10:   ${\mathit{p}}_{cur}\leftarrow {\mathit{p}}_{new}$

11:   $Cos{t}_{cur}\leftarrow Cos{t}_{new}$

12:   $Counter\leftarrow 0$

13:  else

14:   $Counter\leftarrow Counter+1$

15:  end if

16: end while

17: ${\mathit{p}}_{del}\leftarrow {\mathit{p}}_{cur}$

18: return${\mathit{p}}_{del}$

Figure 2 shows an example of the cost map in the workspace around the worker calculated from the HRI constraints. The worker’s shoulder positions (red squares) and the calculated delivery position (green circle) are shown in the figure. We can see that the proposed solver can calculate the delivery position that has the minimum cost in the cost map.

3.3. Worker’s Motion Prediction

The worker’s motion is predicted by using Gaussian Mixture Regression (GMR) proposed in our previous work [34]. GMR models the worker’s past movements and predicts his/her future movements in the workspace. Here, we provide a brief description of the motion prediction using GMR.

Suppose that ${\mathit{p}}_{\mathrm{c}}={\mathit{p}}_{\mathrm{w}}^{\left(t\right)}\in {\mathbb{R}}^n$ is the worker’s current position at time step t, ${\mathit{p}}_{\mathrm{h}}={\left({\mathit{p}}_{\mathrm{w}}^{(t-1)}{\mathit{p}}_{\mathrm{w}}^{(t-2)}\cdots {\mathit{p}}_{\mathrm{w}}^{(t-d)}\right)}^{\mathrm{T}}\in {\mathbb{R}}^{n\times (d-1)}$ is the position history, and d is the length of the position history. GMR models the conditional probability density function $p_r\left({\mathit{p}}_{\mathrm{c}}\right|{\mathit{p}}_{\mathrm{h}})$ whose expectation $E\left[{\mathit{p}}_{\mathrm{c}}\right|{\mathit{p}}_{\mathrm{h}}]$ means the worker’s future position and variance $V\left[{\mathit{p}}_{\mathrm{c}}\right|{\mathit{p}}_{\mathrm{h}}]$ is the uncertainty of the prediction. The details of GMR calculation are shown in Appendix A.

The procedure for the long-term motion prediction using GMR is summarized in Algorithm 2. The calculation to predict the worker’s position at the next time step is repeated until the length of the predicted trajectory becomes equal to the maximum prediction length $T_{\mathrm{p}}$. The worker’s predicted motion is expressed as the sequence of Gaussian distributions $\left({\mathcal{N}}_{\mathrm{w}}^{\left(t_{\mathrm{c}}\right)},{\mathcal{N}}_{\mathrm{w}}^{(t_{\mathrm{c}}+1)},\cdots {\mathcal{N}}_{\mathrm{w}}^{(t_{\mathrm{c}}+T_{\mathrm{p}})}\right)$ starting from the current time $t_{\mathrm{c}}$. $N_{\mathrm{w}}^{\left(t\right)}$ is the worker’s predicted position distribution at step t expressed as:

$$\begin{array}{c}\hfill {\mathcal{N}}_{\mathrm{w}}^{\left(t\right)}=\mathcal{N}({\mathbf{\mu }}_{\mathrm{w}}^{\left(t\right)},{\Sigma }_{\mathrm{w}}^{\left(t\right)})\end{array}$$

(1)

where ${\mathbf{\mu }}_{\mathrm{w}}^{\left(t\right)}$ is the mean vector and ${\Sigma }_{\mathrm{w}}^{\left(t\right)}$ is the covariance matrix of worker’s predicted position at step t.

If the worker repeats his/her normal movement, which is indicated in the process chart of the assembly process, our prediction system can predict the worker’s movement accurately enough for the system. According to our previous research, the RMSE (Root Mean Square Error) of the worker’s movement was about 0.3 m [34]. The RMSE was calculated by the comparison between the initial predicted worker’s movement and the observed worker’s movement when the worker started to move to the next working position.

When the worker moves differently from his/her normal movement, it is not easy to ensure the accuracy of the prediction. However, the proposed system operates safely even in this case, since the variance of the predicted position of the worker is included in the cost function used in the motion planning as shown in our previous study [13].

Algorithm 2: Worker’s motion prediction using GMR

Input: Current time $t_{\mathrm{c}}$,

Current position ${\mathit{p}}_{\mathrm{c}}^{\left(t_{\mathrm{c}}\right)}$,

Position history ${\mathit{p}}_{\mathrm{h}}^{\left(t_{\mathrm{c}}\right)}$,

Max prediction length $T_{\mathrm{p}}$

Output: Predicted trajectory ${\displaystyle \left({\mathcal{N}}^{\left(t_{\mathrm{c}}\right)},{\mathcal{N}}^{(t_{\mathrm{c}}+1)},\cdots ,{\mathcal{N}}^{(t_{\mathrm{c}}+T_{\mathrm{p}})}\right)}$

1: while$k=1to{T}_p$do

2:  ${\mathit{p}}_{\mathrm{h}}^{(t_{\mathrm{c}}+k)}={\left({\mathit{p}}_{\mathrm{c}}^{(t_{\mathrm{c}}+k-1)}{\mathit{p}}_{\mathrm{c}}^{(t_{\mathrm{c}}+k-2)}\cdots {\mathit{p}}_{\mathrm{c}}^{(t_{\mathrm{c}}+k-d-1)}\right)}^{\mathrm{T}}$

3:  ${\mathbf{\mu }}_{\mathrm{w}}^{(t_{\mathrm{c}}+k)}=E\left[{\mathit{p}}_{\mathrm{c}}^{(t_{\mathrm{c}}+k)}\right|{\mathit{p}}_{\mathrm{h}}^{(t_{\mathrm{c}}+k)}]$

4:  ${\Sigma }_{\mathrm{w}}^{(t_{\mathrm{c}}+k)}=V\left[{\mathit{p}}_{\mathrm{c}}^{(t_{\mathrm{c}}+k)}\right|{\mathit{p}}_{\mathrm{h}}^{(t_{\mathrm{c}}+k)}]$

5:  ${\mathit{p}}_{\mathrm{c}}^{(t_{\mathrm{c}}+k)}=E\left[{\mathit{p}}_{\mathrm{c}}^{(t_{\mathrm{c}}+k)}\right|{\mathit{p}}_{\mathrm{h}}^{(t_{\mathrm{c}}+k)}]$

6: end while

3.4. Trajectory Planning and Control

Figure 3 shows the concept of human-following motion planning using the worker’s predicted motion. The sequence of the worker’s predicted position distributions $({\mathcal{N}}_{\mathrm{w}}^{\left(t_{\mathrm{c}}\right)},$${\mathcal{N}}_{\mathrm{w}}^{(t_{\mathrm{c}}+1)},\cdots {\mathcal{N}}_{\mathrm{w}}^{(t_{\mathrm{c}}+T_{\mathrm{p}})})$ is given to the trajectory planner. The sequence of the robot states, that is the robot trajectory $\left({\mathit{q}}^{\left(t_{\mathrm{c}}\right)},{\mathit{q}}^{(t_{\mathrm{c}}+1)},\cdots {\mathit{q}}^{(t_{\mathrm{c}}+T_o)}\right)$, is calculated so that the robot’s state at each time step follows the corresponding predicted position of the worker.

To achieve the prediction-based human-following robot motion, we use an MPC-based planner to consider the evaluation function for finite time future robot states. This is a well-known strategy and is often used in real-time robot applications such as task-parametrized motion planning [35] and multi-agent motion planning [36].

The cost function used in MPC consists of terminal cost and stage cost. The terminal cost deals with the cost at the terminal state of the robot, which is the delivery position in our case. Stage cost considers the state of the robot during the whole trajectory from the current configuration to the goal configuration. A distinct feature of our scheme is that the optimal delivery position, found by optimizing the HRI-based cost function, is used to calculate the terminal cost. In addition, the predicted trajectory of the worker is used to calculate the stage cost. This scheme plans the collision-free robot trajectory that follows the moving worker efficiently under the safety cost constraint and the robot’s velocity and acceleration constraints.

The cost function J used for the optimization of the proposed trajectory planner is expressed as:

$$\begin{array}{c}\hfill J=\phi \left(\mathit{q}(t_{\mathrm{c}}+T_{\mathrm{o}})\right)+{\int }_{t_{\mathrm{c}}}^{t_{\mathrm{c}}+T_{\mathrm{o}}}\left(L_1\left(\dot{\mathit{q}}\left(k\right)\right)+L_2\left(\mathit{q}\left(k\right)\right)+L_3\left(\mathit{q}\left(k\right)\right)\right)dk\end{array}$$

(2)

where $\mathit{q}={(\mathbf{\theta },\dot{\mathbf{\theta }})}^{\mathrm{T}}$$\in {\mathbb{R}}^{2N_j}$ is the state vector of the manipulator, $\mathbf{\theta }={({\theta }_1,{\theta }_2,\cdots ,{\theta }_{N_j})}^{\mathrm{T}}\in {\mathbb{R}}^{N_j}$ is the vector composed of the joint angles of the manipulator, $N_j$ is the degrees of freedom of the manipulator, $T_{\mathrm{o}}(T_{\mathrm{o}}\le T_{\mathrm{p}})$ is the length of the trajectory (in our experiments, we used $T_{\mathrm{o}}=T_{\mathrm{p}}$ as a rule of thumb) and $\phi \left(\mathit{q}(t_{\mathrm{c}}+T_o)\right)$ is the terminal cost which prevents the calculated trajectory of the robot from diverging. It is expressed as:

$$\begin{array}{c}\hfill \phi \left(\mathit{q}(t_{\mathrm{c}}+T_{\mathrm{o}})\right)=\frac{1}{2}{\left({\mathit{FK}}_{N_j}\left(\mathit{q}(t_{\mathrm{c}}+T_{\mathrm{o}})\right)-{\mathit{x}}_{del}\right)}^{\mathrm{T}}R\left({\mathit{FK}}_{N_j}\left(\mathit{q}(t_{\mathrm{c}}+T_{\mathrm{o}})\right)-{\mathit{x}}_{del}\right)\end{array}$$

(3)

where ${\mathit{FK}}_j$ is the forward kinematics of the robot that transform the robot state $\mathit{q}$ from joint coordinates to position ${\mathit{p}}_j$ and velocity ${\mathbf{v}}_j$ in the workspace coordinates. R is the diagonal positive definite weighting matrix. ${\mathit{x}}_{del}$ is the terminal state of the robot which is calculated based on the optimal delivery position and the predicted mean position of the worker. In this study, ${\mathit{x}}_{del}$ becomes ${\mathit{x}}_{del}={\left({\mathbf{\mu }}_{\mathrm{w}}^{(t_{\mathrm{c}}+T_{\mathrm{o}})}+{\mathit{p}}_{del},\mathbf{0}\right)}^{\mathrm{T}}$, where ${\mathbf{\mu }}_{\mathrm{w}}^{(t_{\mathrm{c}}+T_{\mathrm{o}})}$ is the worker’s predicted position at the end of the trajectory $(t_{\mathrm{c}}+T_{\mathrm{o}})$, and ${\mathit{p}}_{del}$ is the calculated delivery position for the worker’s observed position. We calculate ${\mathit{p}}_{del}$ after each sampling interval and assume that the variation in ${\mathit{p}}_{del}$ is negligibly small during the sampling interval of the sensing system, which is 30 ms.

$L_1\left(\dot{\mathit{q}}\left(k\right)\right)$, $L_2\left(\mathit{q}\left(k\right)\right)$ and $L_3\left(\mathit{q}\left(k\right)\right)$ are the stage costs which are expressed as:

$$\begin{array}{cc}\hfill & {\displaystyle L_1\left(\dot{\mathit{q}}\left(k\right)\right)=\frac{1}{2}\sum _{j=1}^N{r}_j(B_{\mathrm{vel},j}\left({\dot{\theta }}_j\left(k\right)\right)+B_{\mathrm{acc},j}\left({\ddot{\theta }}_j\left(k\right)\right))}\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill & {\displaystyle L_2\left(\mathit{q}\left(k\right)\right)=w\sum _{j=1}^{N_j}\frac{1}{D_{\mathrm{M}}\left({\mathit{FK}}_{\mathit{p},j}\left(\mathit{q}\left(k\right)\right),{\mathbf{\mu }}_{\mathrm{w}}^{\left(k\right)},{\Sigma }_{\mathrm{w}}^{\left(k\right)}\right)}}.\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill & {\displaystyle L_3\left(\mathit{q}\left(k\right)\right)=\frac{1}{2}\sum _{j=1}^N({\mathit{FK}}_{\mathit{p},j}\left(\mathit{q}\left(k\right)\right)-{({\mathbf{\mu }}_{\mathrm{w}}^{\left(k\right)}+{\mathit{p}}_{del})}^T)Q({\mathit{FK}}_{\mathit{p},j}\left(\mathit{q}\left(k\right)\right)-({\mathbf{\mu }}_{\mathrm{w}}^{\left(k\right)}+{\mathit{p}}_{del}))}\hfill \end{array}$$

(6)

$L_1\left(\dot{\mathit{q}}\left(k\right)\right)$ is the stage cost to maintain the robot velocity and acceleration within their maximum limits. $B_{\mathrm{vel},j}\left({\dot{\theta }}_j\left(k\right)\right)$ and $B_{\mathrm{acc},j}\left({\ddot{\theta }}_j\left(k\right)\right)$ are defined as:

$$\begin{array}{c}\hfill {\displaystyle B_{vel,j}\left({\dot{\theta }}_j\left(k\right)\right)=\left\{\begin{array}{c}0\left(\right|\left|{\dot{\theta }}_j\right||\le {\dot{\theta }}_{max,j})\hfill \\ {\left(\left|\right|{\dot{\theta }}_j\left|\right|-{\dot{\theta }}_{max,j}\right)}^2\left(\right||{\dot{\theta }}_j\left|\right|>{\dot{\theta }}_{max,j})\hfill \end{array}\right.}\end{array}$$

(7)

$$\begin{array}{c}\hfill {\displaystyle B_{\mathrm{acc},j}\left({\ddot{\theta }}_j\left(k\right)\right)=\left\{\begin{array}{c}0\left(\right|\left|{\ddot{\theta }}_j\right||\le {\ddot{\theta }}_{max,j})\hfill \\ {\left(\left|\right|{\ddot{\theta }}_j\left|\right|-{\ddot{\theta }}_{max,j}\right)}^2\left(\right||{\ddot{\theta }}_j\left|\right|>{\ddot{\theta }}_{max,j})\hfill \end{array}\right.}\end{array}$$

(8)

where ${\dot{\theta }}_{max,j}$ and ${\ddot{\theta }}_{max,j}$ are the maximum velocity and maximum acceleration of the jth joint, respectively.

$L_2\left(\mathit{q}\left(k\right)\right)$ is the stage cost that prevents the robot from hitting the worker. w is a weighting coefficient of this cost function. $D_{\mathrm{M}}(\mathit{x},\mathbf{\mu },\Sigma )=\sqrt{{(\mathit{x}-\mathbf{\mu })}^{\mathrm{T}}{\Sigma }^{-1}(\mathit{x}-\mathbf{\mu })}$ is the Mahalanobis distance that considers the variance of the probabilistic density distribution. Using the Mahalanobis distance between the predicted worker’s position distribution $\mathcal{N}({\mathbf{\mu }}_{\mathrm{w}}^{\left(k\right)},{\Sigma }_{\mathrm{w}}^{\left(k\right)})$ and the end-effector position ${\mathit{FK}}_{\mathit{p},j}\left(\mathit{q}\left(k\right)\right)$ at step k, an artificial potential field is constituted according to the predicted variance. The artificial potential becomes wider in the direction of larger variance in the predicted position.

$L_3\left(\mathit{q}\right)$ is the stage cost to ensure that the robot follows the worker’s motion. Q is the diagonal positive definite weighting matrix. This cost function is responsible for the human-following motion of the robot based on the worker’s predicted trajectory as shown in Figure 3. For each time step of the predicted position distribution of the worker $\mathcal{N}({\mathbf{\mu }}_{\mathrm{w}}^{\left(k\right)},{\Sigma }_{\mathrm{w}}^{\left(k\right)})$, the desirable state of the robot is calculated so that the robot’s endpoint follows the predicted mean position of the worker ${\mathbf{\mu }}_{\mathrm{w}}^{\left(k\right)}$ offset by the calculated delivery position ${\mathit{p}}_{del}$.

Now we can define the optimization problem that will be solved by our proposed system.

$$\begin{array}{cccc}\hfill minimize& \hfill & & J\hfill \\ \hfill subjectto& \hfill & & \dot{\mathit{q}}=f(\mathit{q},\mathit{u})\hfill \\ \hfill & \hfill & & \mathit{q}\left(t\right)={\mathit{q}}_{cur}\hfill \end{array}$$

where f denotes the nonlinear term of the robot’s dynamics, $\mathit{u}$ is the input vector, and $\mathit{q}\left(t\right)$ is the initial state of the trajectory which corresponds to the current state ${\mathit{q}}_{cur}$ of the robot. To solve this optimization problem with the equality constraints described above, we use the calculus of variations. The discretized Euler–Lagrange equations that the optimal solution should satisfy are expressed as:

$$\begin{array}{ccc}& & {\mathit{q}}^{(k+1)}={\mathit{q}}^{\left(k\right)}+f({\mathit{q}}^{\left(k\right)},{\mathit{u}}^{\left(k\right)})\Delta t_{\mathrm{s}},\hfill \end{array}$$

(9)

$$\begin{array}{ccc}& & {\mathit{q}}^{\left(t\right)}={\mathit{q}}_{cur},\hfill \end{array}$$

(10)

$$\begin{array}{ccc}& & {\displaystyle {\mathbf{\lambda }}^{\left(k\right)}={\mathbf{\lambda }}^{(k+1)}-{\left(\frac{\partial H}{\partial \mathit{q}}\right)}^{\mathrm{T}}({\mathit{q}}^{(k+1)},{\mathit{u}}^{\left(k\right)},{\mathbf{\lambda }}^{(k+1)})},\hfill \end{array}$$

(11)

$$\begin{array}{ccc}& & {\mathbf{\lambda }}^{(t+T_o)}={\left(\frac{\partial \phi }{\partial \mathit{q}}\right)}^{\mathrm{T}}\left({\mathit{q}}^{(t+T_o)}\right),\hfill \end{array}$$

(12)

$$\begin{array}{c}\hfill \frac{\partial H}{\partial \mathit{u}}({\mathit{q}}^{\left(k\right)},{\mathit{u}}^{\left(k\right)},{\mathbf{\lambda }}^{\left(k\right)})=0,\end{array}$$

(13)

where H is the Hamiltonian and is defined as:

$$\begin{array}{c}\hfill {\displaystyle H(\mathit{q},\mathit{u},\mathbf{\lambda })=L_1\left(\dot{\mathit{q}}\left(k\right)\right)+L_2\left(\mathit{q}\left(k\right)\right)+L_3\left(\mathit{q}\left(k\right)\right)+{\mathbf{\lambda }}^{\mathrm{T}}f(\mathit{q},\mathit{u})}.\end{array}$$

(14)

The procedure for calculating the online trajectory is shown in Algorithm 3. After the sequential optimization based on the gradient decent, we obtain the optimal trajectory $\left({\mathit{q}}^{\left(t\right)},{\mathit{q}}^{(t+1)},\cdots ,{\mathit{q}}^{(t+T_o)}\right)$. For detailed calculations, please refer to our previous study [13].

Algorithm 3: Robot Trajectory Generator

Input: Target delivery position ${\mathit{p}}_{del}$,

Predicted worker’s trajectory ${\displaystyle \left({\mathcal{N}}^{\left(t\right)},{\mathcal{N}}^{(t+1)},\cdots {\mathcal{N}}^{(t+T_p)}\right)}$,

Current state of the robot ${\mathit{q}}_{cur}$,

Max length of the robot trajectory $T_{\mathrm{o}}$

Output: Optimal trajectory is $\left({\mathit{q}}^{\left(t\right)},{\mathit{q}}^{(t+1)},\cdots {\mathit{q}}^{(t+T_o)}\right)$

1: Initialize the set of input vectors $\mathit{u}$

2: ${\mathit{q}}^{\left(t\right)}\Leftarrow {\mathit{q}}_{cur}$

3: while${\Sigma }_{k=t}^{t+T_o}\left|\frac{\partial H}{\partial \mathit{u}}({\mathit{q}}^{(k+1)},{\mathit{u}}^{\left(k\right)},{\mathbf{\lambda }}^{(k+1)})\right|<ϵ$do

4:  while $k=1to{T}_o$ do

5:   ${\mathit{q}}^{(t+k)}\Leftarrow {\mathit{q}}^{(t+k-1)}+f({\mathit{q}}^{(t+k-1)},{\mathit{u}}^{(t+k-1)})\Delta t_{\mathrm{s}}$

6:  end while

7:  while $k=T_{o}to1$ do

8:   ${\displaystyle {\mathbf{\lambda }}^{(t+k-1)}\Leftarrow {\mathbf{\lambda }}^{(t+k)}-{\left(\frac{\partial H}{\partial \mathit{q}}\right)}^{\mathrm{T}}({\mathit{q}}^{(t+k)},{\mathit{u}}^{(t+k)},{\mathbf{\lambda }}^{(t+k+1)})}$

9:  end while

10:  while $i=1to{T}_o$ do

11:   $s_i\Leftarrow {\left(\frac{\partial H}{\partial \mathit{u}}\right)}^{\mathrm{T}}({\mathit{q}}^{(t+i-1)},{\mathit{u}}^{(t+i-1)},{\mathbf{\lambda }}^{(t+i)})$

12:  end while

13:  $\mathit{u}\Leftarrow \mathit{u}+c\mathit{s}$

14:  end while

15:  while$k=1to{T}_o$do

16:   ${\mathit{q}}^{(t+k)}\Leftarrow {\mathit{q}}^{(t+k-1)}+f({\mathit{q}}^{(t+k-1)},{\mathit{u}}^{(t+k-1)})\Delta t_{\mathrm{s}}$

17:  end while

4. Experiment

4.1. Experimental Setup

To evaluate the performance of the proposed scheme in a real-world environment, we used the planar manipulator PaDY proposed in our previous study [7]. PaDY was designed to assist the workers of an automobile factory. A parts tray and a tool holder were attached to the end-effector of PaDY to store the parts and tools required for car assembly tasks. The robot delivers the parts and tools to the worker during the assembly process. For the details of the hardware design of PaDY, please refer to [7].

The proposed scheme was installed in a computer with an Intel Core i7-3740QM (Quad-core processor, 2.7 GHz) with 16GB memory. All calculations were done within 30 ms, the sampling interval of the sensing system that tracks the position of the human worker.

We designed an experiment to demonstrate the effectiveness of the worker’s motion prediction in the human-following behavior of our proposed scheme. Figure 4 shows the experimental workspace and Figure 5 shows the top view of the setup for this experiment. In this experiment, the worker needs to perform the following six tasks:

1

Tightening a bolt (Task 1);

2

Attaching three grommets (Task 2);

3

Attaching one grommet (Task 3, Task 4, Task 5, Task 6).

Each task is performed at a separate working position in the workspace. The experiment is carried out as shown in Figure 6. The experiment is carried out as follows.

1

The experiment begins when the robot starts to approach the worker standing at the working position for Task 1. The worker takes a bolt and the bolt tightening tool from the robot (Figure 6a).

2

The worker performs Task 1 (Figure 6b).

3

The worker moves to the working position for Task 2 and the robot follows him. The worker returns the bolt tightening tool to the tool holder (Figure 6c) and picks up three grommets from the parts tray.

4

The worker performs Task 2 (Figure 6d).

5

The worker moves to the working position for Task 3 and picks up a grommet from the tray (Figure 6e).

6

The worker performs Task 3 (Figure 6f).

7

The worker moves to the working position for Task 4 and picks up a grommet from the tray (Figure 6g).

8

The worker performs Task 4 (Figure 6h).

9

The worker moves to the working position for Task 5 and picks up another grommet from the parts tray (Figure 6i).

10

The worker performs Task 5 (Figure 6j).

11

The worker moves to the working position Task 6 and picks up the last grommet from the parts tray (Figure 6k).

12

The worker performs Task 6 (Figure 6l) and this concludes the experiment.

We performed this experiment with four different participants (A, B, C and D) to evaluate the robustness of the system for different workers. Each participant is asked to perform the complete work cycle ten times. The first trial is performed without using the predicted motion of the worker. Whereas, in all other trials, the predicted motion of the worker is used and the worker model is sequentially updated after completing each trial.

For more details about the experiment, please see the Supplemental Materials.

4.2. Tracking Performance

Figure 7a shows the estimated delivery position and the robot’s end-effector position for trial 1 of a participant when the robot’s motion is calculated based on the observed position of the worker without using the motion prediction. The black vertical lines show the time when the worker performs each assembly task. Figure 7b shows the estimated delivery position and the robot’s end-effector position for trial 10 of the same participant when the robot’s motion is calculated based on the predicted position of the worker using the proposed scheme.

At the beginning of the experiment, the robot is at its home position and the participant is at the working position for Task 1. The robot starts its human-following motion after arriving at the delivery position for Task 1 (at around 6 s in Figure 7a,b). We can see that the robot keeps following the participant during the whole experiment in both schemes (with and without the use of motion prediction).

The green line in Figure 7a,b shows the tracking error which is the difference between the delivery position and the end-effector position. We can see that the maximum tracking error is reduced from about 0.5 m to 0.3 m by using the motion prediction.

It is not possible to completely eliminate the tracking error since the manipulator used for the experiments has a mechanical torque limiter at each joint and the maximum angular acceleration without activating the torque limiter is 90 deg/s ${}^2$. In both Figure 7a,b, a large tracking error around 30 s can be observed. This is because the participant makes a large movement around 30 s and the robot cannot follow the participant because of its acceleration limit.

4.3. Cycle Time

Figure 8 shows the comparison of the cycle time of the four participants in each trial. We define cycle time as the time required for a participant to complete all six tasks of the assembly process. In Figure 8, the cycle time of each trial is normalized by the time of trial 1. Remember that motion prediction was not used in trial 1.

We can see that the cycle time for each participant decreases as the number of trials increases. The cycle time of trial 10 is reduced to 65.6–74.8% of the cycle time of trial 1. This shows that motion prediction can improve the performance of participants and help them complete the assembly process faster.

Note that the proposed system ignores the dynamics of the interaction between the robot and the worker, assuming that the worker is well trained and the behavioral dynamics of the worker with respect to the robot’s movements can be ignored. If the effects of the robot’s motion on the worker can be modeled, the system can better deal with the effect of the interaction between the worker and the robot and a further improvement in the worker’s time efficiency could be expected.

4.4. HRI-Based Cost

Table 1. Summary of HRI-based costs during the human-following motion for each worker.

Worker	Average Cost (without Prediction)	Average Cost (with Prediction)	Max Cost (without Prediction)	Max Cost (with Prediction)
Worker A	8.99	11.79	36.34	35.82
Worker B	12.73	9.90	38.17	34.44
Worker C	18.35	13.56	39.65	31.33
Worker D	16.30	17.50	31.47	31.26

shows the average and maximum HRI-based costs for each participant during the human-following motion of the robot. Since the HRI-based cost increases as the safety and comfort of the worker decreases, it is desirable to have a low HRI-based cost in human–robot collaboration.

In Table 1, we see that there are no significant differences in the average and maximum HRI-based costs between trial 1 (when motion prediction is not used) and trial 10 (when motion prediction is used) for all four participants. Therefore, we conclude that the proposed prediction-based human-following control reduces the work cycle time without adversely affecting the safety and comfort of the workers.

5. Conclusions

We proposed a human-following motion planning and control scheme for a collaborative robot which supplies the necessary parts and tools to a worker in an automobile assembly process. The human-following motion of the collaborative robot makes it possible to provide anytime assistance to the worker who is moving around in the workspace.

The proposed scheme calculates an optimal delivery position for the current position of the worker by performing non-convex optimization of an HRI-based cost function. Whenever the worker’s position changes, the new optimal delivery position is calculated. Based on the observed movement of the worker, the motion of the worker is predicted and the robot’s trajectory is updated in real-time using model predictive control to ensure a smooth transition between the previous and new trajectories.

The proposed scheme was applied to a planar collaborative robot called PaDY. Experiments were conducted in a real environment where a worker performed a car assembly process with the assistance of the robot. The results of the experiments confirmed that our proposed scheme provides better assistance to the worker, improves the work efficiency, and ensures the safety and comfort of the worker.

This scheme has been designed for a single worker operating within his/her workspace. It is not designed for the cases when multiple workers are operating in the same workspace, or when the worker moves beyond the workspace. Moreover, we did not consider the dynamics of interaction between the robot and the human, assuming that the human worker in the factory is well trained and his/her behavior dynamics to the robot motion is negligible. If the effects of the robot’s motion on the human can be modeled, the system can better deal with the effect of the interaction and further improvement in time efficiency could be expected.

We believe that the human-following approach has tremendous potential in the field of collaborative robotics. The ability to provide anytime assistance is a key feature of our proposed method, and we believe it will be very useful in many other collaborative robot applications.