Systems involving collaboration between humans and robots have the potential to reduce the workload on human operators through robotic assistance significantly. The ability of such human-assistive autonomous robots to adapt to unpredictable human preferences while optimizing energy usage is a critical factor in achieving operational efficiency and ensuring safety [1]- [3]. In Human-Robot Collaboration (HRC), a common task is object co-manipulation [4], [5]. However, utilizing such co-manipulation introduces significant challenges that include the unpredictability of human actions [6], which may deviate from established paths, and the increased complexity in controlling the system, because of the high degree of freedom of a robotic arm [7].
To address these issues, this paper develops a control algorithm by formulating and solving a human uncertaintyaware Model Predictive Control (MPC). The modeling of This project is supported by NSF EPCN Grant #2332210. human uncertainty in MPC tracking allows us to estimate its impact on costs in terms of tracking errors and energy consumption when performing manipulation tasks.
Literature review: Tracking problems arise from planning and control, where primary efforts are based on the platform of mobile robots. For ground vehicles, Ostafew et al. [8] developed a learning-based nonlinear MPC designed for navigating off-road terrains. Xiao et al. [9] introduced an imitation learning-based MPC aimed at facilitating navigation in crowded public spaces. Additionally, notable advancements have been made in the field of adaptive planning and control strategies [10]- [13]. For 3D space, various approaches have been developed for UAV platforms [14], where environmental features are less critical, while the vehicle dynamics becomes more sophisticated. These approaches enhance the robot's adaptability and responsiveness to dynamics and environmental changes.
In recent years, using robotic arms for end-effector trajectory tracking has attracted a lot of research interest. Baek et al. [15] presented a practical adaptive time-delay control applied in robot manipulators, Xu et al. [16] developed a learning from demonstration method for robotic arm trajectory tracking, and model predictive path-following control by Faulwasser et al. [17], focusing on precision and efficiency in arm movements. There also exist studies that focus on integrating robotic bases with arms for enhanced tracking performance. Quiroz-Omana et al. [18] proposed a robot kinematic control strategy using linear programming. Giftthaler et al. [19] solved a constrained sequential linear quadratic control problem to design a time-varying feedback controller. Osman et al. [20] used the whole body dynamics to obtain a task-space stabilization controller utilizing nonlinear MPC.
However, a common oversight in these studies is neglecting external uncertainties, especially those arising from human involvement in collaborative tasks. While robust MPC addresses uncertainties within the robot's control framework [21], [22], the focus is mainly on the robot's internal dynamics [23], [24], which are not directly applicable to the characterization of external uncertainty.
Statement of contribution: In this paper, we study humanrobot co-manipulation using a model of a robot manipulator. The main contributions of this work are as follows: First, we model human uncertainties that follow a normal distribution with non-zero mean and variance. We then formulate a new human uncertainty-aware MPC tracking problem. Then, we solve this problem to get the optimal control input sequence, i.e., the manipulator's joint angle velocities. Finally, we verify our approach's effectiveness using theoretical derivation and simulated experiments. We compare its performance with a baseline algorithm that does not consider human uncertainty while solving for optimal control law.
Notations: Let I r denote the r × r identity matrix. Let diag{a 1 , a 2 , • • • , a r } denote a diagonal matrix with a i being the ith diagonal entry. For a vector x, |x| 2 denotes its 2-norm. For a square matrix M , Tr(M ) denotes its trace. We use M ≻ 0, M ⪰ 0 to denote the matrix is positive definite and positive semi-definite, respectively. We let ∥x∥ 2 M = x ⊤ M x with M ⪰ 0, which represents a quadratic function of the vector evaluated by the matrix M .
This section formulates our problem of interest. We introduce a trajectory around a fetch robot with human uncertainties that the robot needs to track and adapt to. Then, we describe how we model human uncertainty and the robot's motion dynamics. Finally, we formulate a human uncertaintyaware MPC tracking problem to describe the control objective mathematically.
We aim to perform a co-manipulation task between a robot and a human by balancing a board, as illustrated in Fig. 1. To this end, we define a nominal trajectory that represents the robot's end-effector's expected position and orientation in the world frame, which is defined as
where superscripts x, y, z represent the 3D positions and α, β, γ represent the roll, pitch, and yaw orientations, respectively. As we are balancing the board, the pitch (β) and yaw (γ) should always be kept zero. We assume that human attempts to follow the nominal trajectory, but their actions are subject to uncertainties. This will lead to a deviation of the board from the nominal trajectory, necessitating the robot to adjust accordingly. To model this, we consider a receding horizon tracking problem [25]. At each time step t, the robot seeks to track a segment of the trajectory for future H steps:
with k ∈ {0, 1, • • • , H} and ϖ ∈ R 3 represents the human uncertainties modeling discussed in section II-B. D = I 3 0 3×3 ⊤ ∈ R 6×3 is a matrix that maps human uncertainties onto the nominal trajectory. We assume that the human only causes positional uncertainties, without affecting the desired roll, pitch, and yaw of the reference trajectory. Therefore, the last three rows of D are kept as zeros. The vector h = [h x , h y , h z ] ⊤ ∈ R 3 represents the positional uncertainty created by the human up to time step t, which can be directly observed by the robot and should be added to the nominal trajectory.
In our approach, we model human behavior and preferencebased uncertainty as ϖ(t) ∼ N (µ, Σ). µ ∈ R 3 is the tendency or the average expected positional uncertainty and the covariance matrix Σ ∈ R 3×3 , oriented in the x, y, z directions, captures the variability and individual differences in human behavior, providing insight into how uncertainties might deviate from the mean in three-dimensional space. To quantify such uncertainty, suppose a human trajectory can be measured as r(t). Comparing it with the nominal trajectory, the positional difference is defined as
It is easy to formulate the following minimization problem to estimate E(ϖ) = µ,
To further quantify Σ, assuming the solution of Equation ( 3) to be μ, then we can estimate the covariance matrix as,
Here, the factor of 1 T -1 is due to the concept of Bessel's correction [26]. Now, to track the trajectory with such uncertainty, i.e., Equation (1), in a receding horizon manner, we introduce the dynamics of the mobile manipulator in the following subsection.
The Fetch robot features a 7-DOF (degrees of freedom) robotic arm built on a robotic base [27]. We treat the base's heading angle as an additional degree of freedom for the arm. With this adjustment, we equivalently have an 8-DOF robotic arm, as depicted in Figure 2 [28]. To get the state transition of the 8-DOF arm, we use forward kinematics using Jacobian Fig. 2: Joint axes configurations of the 8-DOF configuration with DH parameters in body frame Ξ within the inertial frame. matrix J(•) ∈ R 6×8 derived based on the DH-parameters [29] of the robotic arm configured in Fig. 2:
where s represents the end-effector pose, θ
represents the corresponding joint angle velocities. Particularly, since θ 1 is the rotation of the base, its actuation is based on a differential signal on its wheels v a and v b . J(θ(k)) = ∂f (θ) ∂θ ∈ R 6×9 is the Jacobian matrix. B(θ(k)) = τ J(θ(k)) is the input matrix which depends on the joint angle combinations θ(k).
Our problem of interest is to track the trajectory (1) by the end-effector of the robot with the 8-DOF robotic arm dynamics (5). We solve this by introducing a unique MPC formulation, where we add an extra variable ϖ that represents human uncertainties as discussed in section II-B and formulated as Equation (6). We solve this equation for optimal control input sequence over the planning horizon. Note that, we do not consider uncertainties coming from the robot dynamics, rather our formulation considers human-induced uncertainties.
Our proposed human uncertainty-aware MPC tracking can be formulated as follows:
where u(0 :
≻ 0 are the weighting matrices for tracking and input costs, respectively. s 0 is the starting end-effector pose of a planning horizon. Without considering human uncertainty, i.e., µ = 0 and Σ = 0, problem (6) becomes a regular MPC tracking problem.
III. MAIN RESULT While addressing the problem (6), it is obvious that the optimal control input sequence u(0 : H -1) depends on the input matrix B(θ). This section presents a solution to obtain the optimal control input sequence u(0 : H -1). Our approach is summarized in Algorithm 1. We define the following error dynamics for (5) by subtracting r(k + 1) from both sides of the equation:
with e(k) = s(k) -r(k) being the tracking error. We assume that the optimal cost-to-go function follows:
where, P (k) ∈ R 6×6 , p(k) ∈ R 6 , c(k) ∈ R, are unknown matrices, vectors, and scalars to be determined. The optimal control input sequence u * and Theorem (1) can verify the assumed solution.
Theorem 1. Assuming the optimal solution u * of (6) yields an optimal cost J * with the form of (8). Then P (k), p(k), and c(k) can be computed by the following ua-DARE:
with M = (R + B ⊤ P (k + 1)B) -1 B ⊤ , terminal conditions:
Based on Theorem (1), for each time-step k, the optimal control inputs follow:
Proof. For derivation simplicity, let B = B(θ). Integrating Equation ( 7) into the optimal cost Equation ( 8),
where E(ϖ(k + 1)) = µ. The optimality condition Thus, for each time-step k, the control inputs follows:
with M defined in Theorem (1). Bringing this back to ( 14) yields
Reusing condition (15) by left multiplying u(k) ⊤ , using u(k) from ( 16), and bringing them into (17) yields:
[Q+P (k+1)-P (k+1)BM P (k+1)]
+ 2e(k) ⊤ p(k + 1) -P (k + 1)B(M p(k + 1) -P (k + 1)BM P (k + 1)(r(k) -r(k + 1) -Dµ)
Comparing ( 18) with ( 8), we have the DARE for P (k), p(k), and c(k). The terminal conditions are obtained by considering J * (e(H), k = H) for ( 6), where P (H) = Q Algorithm 1: Biased Human-Uncertainty-Aware MPC Tracking
This section validates the proposed human uncertaintyaware MPC tracking algorithm using simulated experiments. We build the model of a Fetch robot, shown in Fig. 2, using methods described in Sec. II with DH parameters [30] for link lengths, offset values, and rotation twist angles.
To randomly generate a human trajectory, then estimate its uncertainty as ϖ(k) ∼ N (µ, Σ), where µ = [0.0052, 0.0047, 0.0021] ⊤ (meters), Σ = diag(0.014, 0.025, 0.010) (meters). It characterizes a human who tends to create more uncertainties in the y-axis than x, z-axes. For comparison purposes, we articulately increase the magnitude of the covariance matrix by a factor of q ∈ {1, 2, 4} with q = 1 representing the base case, and 2 and 4 increases the strength of the uncertainties.
We validated our proposed algorithm 1 by performing tracking with three different trajectories that are subject to human uncertainties. The decomposed x, y, and z axes for each trajectory are visualized in Fig. 3. Each trajectory is TABLE I: Total Cost Comparison of Our Approach with Baseline Algorithm Traj Q q BHU N-BHU A 10000I 6 1 587.945 1728.750 2 1458.788 2682.087 4 2878.625 4097.953 5000I 6 1 377.683 913.213 2 842.449 1482.981 4 1462.193 2138.699 B 10000I 6 1 1289.893 2010.958 2 2145.533 3086.132 4 3712.969 4912.039 5000I 6 1 963.029 1544.563 2 1372.532 2036.649 4 2125.284 2893.756 C 10000I 6 1 736.422 1294.321 2 1649.417 2430.971 4 3243.415 4311.028 5000I 6 1 547.486 944.716 2 1009.714 1501.895 4 1797.617 2420.096 discretized into 400 points with a time step τ = 0.1 (sec). Throughout the simulation, we use real physics to update the robot's states and calculate the end-effector pose with forward kinematics. The parameters are chosen as H = 5, R = I 9 , We set either Q = 10000 • I 6 or Q = 5000 • I 6
We repeat the MPC planning every step, compute the optimal control inputs for the planning horizon, and update the robot's state with the first control input. To evaluate the control performance, we define the total cost
taking into account the robot's end effector tracking error cost and control input cost. Fig. 3 shows the tracking visualization for each trajectory. we compare our proposed approach, MPC tracking with consideration of biased human uncertainty (BHU), with a baseline algorithm that does not consider any human uncertainty while tracking (N-BHU). We observe that the dots representing tracking with BHU (Red) are comparatively closer to the dots of nominal trajectory with human uncertainties (Blue) than the dots of tracking with N-BHU (Green). As we have a relatively larger value for Q, we observe that tracking dots along all three x, y, and z-axes for every trajectory align closely with the trajectory to be followed.
Table I shows the performance in terms of cost for all three trajectories with different Q and q values. The comparison shows that our approach, BHU, outperforms N-BHU. Additionally, we observe a gradual increase in total cost corresponding to the increase in q. This phenomenon occurs as the robot attempts to counteract human uncertainties. However, its ability to fully mitigate these uncertainties is limited due to the small time step, τ . Consequently, the tracking error increases, contributing to the increment in total cost.
The cost plots, also illustrated in Figure 3, show the variation in cost at each time step for all three trajectories. It is worth mentioning that in almost every step, BHU has a lower cost than N-BHU. In some time steps, the cost difference is noticeably higher than in other time steps. This observation can be related to the trajectory itself, meaning that, while navigating complex trajectory segments, BHU is more cost-efficient. This consistent cost advantage translates into the superior overall performance of our proposed method, as detailed in Table I.
We developed a novel control algorithm for human-robot co-manipulation tasks utilizing an 8-DOF robotic arm. We explicitly model human uncertainty as a normal distribution with non-zero mean and covariance. We insert that as an extra variable in our formulation, leading to an uncertainty-aware MPC formulation. We solved for the optimal control law from this formulation by computing uncertainty-aware DARE theoretically. We later validated our approach by simulating using a Fetch robot model. Our proposed approach outperforms a baseline algorithm that does not consider human uncertainty in tracking accuracy and energy consumption. Future work will include implementing our algorithm in the Gazebo simulator and on a physical Fetch robot platform.