Model Predictive Impedance Control with Gaussian Processes for Human and Environment Interaction

An important challenge in physical HRI is the ability of robots to infer human co-worker’s goals and states. To address this challenge, several recent works have proposed inference techniques and applications. The study in [15] showed that robot adaptation to the human task execution can improve task-related performance. Robot adaption to the human state can also improve ergonomics [11]. Finally, user satisfaction can also be improved by robot adaptation [16]. Nevertheless, to adapt to these different aspects, the robot should be able to infer the human state, which often requires complex sensory setups [17, 18]. However, complex measurement systems are not always available or feasible for practical applications, thus some specific states can be modeled, such as the desired motion of the robot/payload [19, 20, 21, 22], human actions [23], preferences [24, 25], human physical fatigue [11], cognitive stress [26], and suitability of shared workspace [27, 28, 29, 30].

For seamless collaboration, the adaption of the robot to the human should be prompt, and thus the human intent or goal must be inferred during the task execution [31, 11]. Human intent is often considered as a continuous variable [15, 22], however, it can also reflect discrete changes in the task [32], e.g., a collection of possible goals. Discrete human state has been considered for virtual fixtures [33], Dynamic Movement Primitives (DMPs) [32], and impedance control [34].

When the robot has the knowledge of the human state and intent, it must respond promptly with appropriate actions to facilitate the collaborative task execution. This typically means generation of motion trajectories and often simultaneous impedance adaptation. The trajectories can be chosen to optimize task-related objectives: reducing trajectory jerk [35, 36], minimize positioning error [8, 37, 38], or render appropriate velocity response [39, 40]. In addition, human operator-related objectives can be optimized as well, such as minimizing interaction forces [41] or metabolic cost [42]. A common rule is that the robot can take over aspects of the task that require precision, while the human can handle adaption to variations. The ‘minimum intervention principle’ [43] follows this rule, where uncertain DOF should have a lower stiffness [44, 45], which can also be interpreted as a risk-sensitive control. Nevertheless, such an approach is not reasonable for all interactive tasks that involve physical constraints [7].

The literature can address the human-related properties (e.g., actions, preferences, ergonomics, etc.), real-time intention processing, and uncertainty in an isolated manner. However, a unified framework that can effectively combine all these aspects for practical manufacturing tasks is still missing.

Physical HRI requires a safe and robust low-level control approach to account for the uncertainty of practical tasks [57]. Classic position or force controllers often fail to perform well in such complex settings since they only focus on controlling either one of the variables, but not the characteristics of the interaction that define their relationship, i.e., impedance. Impedance adaption is empirically important for safety and robustness in manipulation, and has been shown to improve sample efficiency of higher-level learning control [58]. Accordingly, a wide range of techniques has been proposed for adjusting impedance parameters. Recent surveys provide a thorough overview of this field [59, 60], so we present only an abbreviated summary in Table 1.

Dynamics-based objectives are often based on continuous-time dynamic models, considering factors like stability and damping of the coupled system. A challenge is that environment dynamics (e.g., stiffness) are application-specific. In some cases, the environment dynamics are known or controlled [47], in other cases properties of the dynamics (e.g., damping) can be found from trajectories [61].

Other approaches plan to minimize a general cost function, which may include the human or environment state [53]. Such approaches allow consideration of uncertainty, but typically also require models of humans or the environment, which are typically simplified such as assuming human force is a PD controller [43]. Data-driven methods such as reinforcement learning can be applied [54, 55], but often require lots of trial and error.

An approach in [4] uses GP models to encode demonstrated desired robot movements and then leverages on the measure of uncertainty in the modeled skill to modulate the stiffness through a direct expression. If the robot experiences unexpected external perturbations, the impedance controller forces the motion into the regions of higher certainty to be more confident in the given skill. If the robot still ends up in a region of high uncertainty, the stiffness is reduced as there might be insufficient confidence in the skill to safely perform the movements there. Nevertheless, there was no higher-level planner (e.g., MPC) to exploit given learned trajectories to generate new ones.

From this, we conclude that there is a wide range of objectives and models–it seems unlikely that a single principle would be suited for all or a large majority of these areas. Accordingly, we propose a framework for optimizing impedance, capable of accommodating a range of models and objectives.

Intention modeling and low-level impedance control alone can work well for simplified tasks. However, for a more flexible robot behavior, a higher-level planner is required. MPC is becoming increasingly popular for manipulation [62, 13], and is already a standard tool for locomotion and whole-body control, capable of planning through contact models with either linear complementarity [63] or switching dynamic modes [64]. Contact can be considered as fully rigid constraints, using time-stepping approaches to resolve the contact impulse over an integrator time step [65], avoiding the issue of unbounded contact forces in rigid contact. A dynamic model for the environment has also been proposed for manipulation [66], relaxing kinematic constraints into a stiffness. In most contact planning situations, signed distance functions are needed–typically, this requires geometry information typically given by CAD, although it has been learned on simplified objects [67].

We make two observations from the perspective of manipulation: 1) signed distance functions are difficult with the complex geometries typical in assembly and manipulation tasks, and 2) compliance is both common and critical–be it physical or by control. To address this, we use a compliant contact model here (regressing contact force over robot position) which does not require a signed distance function, and we consider the optimization of impedance parameters in the MPC problem.

For a robot with a tool-center point (TCP) pose $\bm{x}$, we consider three types of external forces:

We use one force/torque (F/T) sensor, such that demonstrations measure total force $\bm{f}=\bm{f}^{h}+\bm{f}^{c}+\bm{f}^{d}$. While multiple F/T sensors can separate human and environment forces [56, 55], a single F/T sensor reduces complexity. A disadvantage of a single F/T sensor is that in co-manipulation with contact, contact may be more difficult to identify, although this can be partly addressed by how the data is collected. Another disadvantage of this approach is that forces are regressed over position, which may be challenging in stiff contact. We validate both these aspects in Section 5.3 and Fig. 5.

This subsection develops the dynamics of a robot with a Cartesian admittance controller. An admittance is used as the hardware implementation here uses a position-controlled industrial robot. As impedance and admittance are just inverse mathematical viewpoints of the interactive dynamics, the dynamics here can be readily adapted to impedance-controlled robots. Consider a rendered admittance in the TCP frame which approximates the continuous-time dynamics of

with pose $\bm{x}\in\mathbb{R}^{6}$, velocity $\dot{\bm{x}}\in\mathbb{R}^{6}$, acceleration $\ddot{\bm{x}}\in\mathbb{R}^{6}$, measured force $\bm{f}\in\mathbb{R}^{6}$ and force reference $\bm{f}^{r}$. The impedance parameters are inertia $\bm{M}\in\mathbb{R}^{6\times 6}$, damping $\bm{D}\in\mathbb{R}^{6\times 6}$, and stiffness $\bm{K}\in\mathbb{R}^{6\times 6}$ matrices, which are here all diagonal, and $\bm{x}_{0}$ which is the rest position of the spring. The rotational elements of $\bm{x}$ and $\bm{f}$ are the angles and torques, respectively, about the three axes of the TCP frame. In the sequel, $\bm{K}$ and $\bm{x}_{0}$ are dropped as it was found empirically that the quasi-static tracking can be handled by the MPC adjusting $\bm{f}^{r}$.

Ergonomic costs are more naturally expressed in terms of human joint torques [18, 30], so we also consider a 4-DOF kinematic model of the human arm with three rotational DOF at the shoulder and one rotational joint for the elbow extension. This model has parameters of $l_{1}$ and $l_{2}$, for the length of the upper and lower arm, and $\bm{x}^{sh}\in\mathbb{R}^{3}$ for the spatial position of the human shoulder. Using these parameters and the human joint angles $\bm{q}\in\mathbb{R}^{4}$, we can calculate the human hand position $\bm{x}^{H}\in\mathbb{R}^{3}$ with forward kinematics as

Forces measured at the end-effector can be translated into human joint torques $\bm{\tau}_{H}$ as

where $\bm{f}_{H}\in\mathbb{R}^{3}$ are linear forces at the human hand, and $\bm{J}_{H}(q)\in\mathbb{R}^{3\times 4}$ is the standard Jacobian matrix of (10) with respect to $\bm{q}$.

Task-specific knowledge can be used to model $\bm{x}^{H}$ or $\bm{q}$. In most cases, we simplify the human kinematics by assuming the internal/external rotation of the arm is zero, $q_{3}=0$, and that $\bm{x}^{H}=\bm{x}$, i.e., the human hand is at the robot TCP, allowing us to write closed-form inverse kinematics and evaluate (11).

The GPs are then used in a Bayesian estimate of the current mode, where a belief at time $t$ is denoted as $\bm{b}_{t}\in\mathbb{R}^{N}$, $\bm{b}_{t}[n]=p(n|\bm{f}_{1:t},\bm{x}_{1:t})$, and $\bullet_{1:t}=[\bullet_{1},\dots,\bullet_{t}]$. This belief is updated recursively as

where $\bm{\mu}^{f}_{n}$ and $\bm{\Sigma}^{f}_{n}$ are the mean and covariance of the GP model in mode $n$ evaluated at $\bm{x}_{t}$, and $\bm{b}_{t}$ is normalized after the update such that $\sum\bm{b}_{t}[n]=1$.

The MPC problem is formulated using multiple-shooting transcription with a generic problem statement of

where $H$ is the planning horizon, $U$ is the range of allowed inputs, $\varrho$ the slack for the continuity constraints (the inequality is applied element-wise), $f$ is from (7), $g$ following (9), and $\bm{h}$ are inequality constraints to be presented in 4.3. The cost function $c_{n}$ is shown with it’s parameter argument $\xi_{t}$ and decision variable $\bm{u}$, the detailed cost function provided in (4.2.1). Initial covariance is set $\Sigma_{t}=0$ as it’s assumed that the robot state is fully observed.

The constraints in (13) are nonlinear, so an interior-point nonlinear optimization solver is used. While nonlinear, the problem is writtein in an automatic differentiation framework, allowing calculation of the gradient and Hessian of the objective and constraints, significantly improving convergence.

The MPC framework here allows for different choices of decision variable $\bm{u}$, which can include any of the following:

where $\bm{\Delta}^{M}_{t}$ and $\bm{\Delta}^{D}_{t}$ are a change in the impedance parameters $\bm{M}$ and $\bm{D}$.

Impedance parameters affect contact safety. To improve contact safety, constraints for well-damped contact and force limit are formulated.

Why should we optimize impedance parameters $\phi$ instead of just the trajectory $\bm{\mu}\bullet$? Taking the differential of the linear-Gaussian dynamics (7) and (9),

where prefix $d$ indicates total differential and $d_{\bullet}$ the partial derivative with respect to $\bullet$. We note two properties: i) input $\bm{f}^{r}$ doesn’t affect the covariance $\bm{\sigma}^{+}$, and ii) state $\bm{\mu}^{+}$ is determined by both control input $\bm{f}^{r}$ and impedance parameters $\phi$, i.e., different combinations of force and impedance parameters can realize a certain change in $\bm{\mu}^{+}$, as previously noted in [54].

When the robot increases stiffness, the mean and variance of the robot’s trajectory are affected, for example reducing response to force disturbances. However, changing the robot’s force reference only affects the mean trajectory. This shows that explicit consideration of trajectory covariance is important for impedance, thus the MPC trajectories should be stochastic when optimizing impedance.

For the optimization problem to be efficient, the objective should be convex with respect to the decision variables [76]. As optimizing with respect to parameters of the dynamics (e.g., impedance parameters) is not studied, we check the convexity of a toy cost function with respect to impedance parameters, denoted $\phi=[\bm{M},\bm{D}]$. As impedance parameters $\phi$ mostly affect covariance, we consider a simplified objective in a single step shooting problem of $\min_{\phi}\mathrm{Tr}(\bm{Q}\bm{\Sigma}^{+})$, where by results from [69],

The convexity of this objective will depend on the impedance parameter values, the choice of the integrator (which affects $\bm{A}$ and $\bm{B}$), and the relative magnitude of $\bm{\sigma}$ and $\bm{\sigma}^{F}$. Note that $\bm{B}^{T}\otimes\bm{B}^{T}=[0,0,0,T_{s}\bm{M}^{-2}]$, which gives $\nabla_{\bm{M}}^{2}\bm{B}^{T}\otimes\bm{B}^{T}\succeq 0$. Thus increasing $\bm{\Sigma}^{f}$ can increase the minimum eigenvalue of the Hessian of the objective (5.2). The terms for damping are more complex, thus numerical analysis is carried out.

These results are validated numerically to see what matrix cost and integrator have better convexity. The minimum eigenvalue of the Hessian $\nabla^{2}_{\phi}\mathrm{Tr}(\bm{Q}\bm{\Sigma}^{+})$ and $\nabla^{2}_{\phi}\ln\det(\bm{Q}\bm{\Sigma}^{+})$ are compared for three different integrator schemes: explicit Euler $\dot{x}^{+}=(1+T_{s}D/M)\dot{x}$, implicit $\dot{x}^{+}=M(M+T_{s}D)^{-1}\dot{x}$, and exponential $\dot{x}^{+}=\exp(-T_{s}D/M)\dot{x}$. We can see in Fig. 4 that the minimum eigenvalue generally increases as $|\bm{\Sigma}^{f}|$ increases. Using a $\mathrm{Tr}(\bm{Q}\bm{\Sigma}^{+})$ is unilaterally better than $\ln\det(\bm{Q}\bm{\Sigma}^{+})$, and is thus used in the cost function here. The implicit integrator has better average performance over the range of $\bm{\Sigma}^{f}$—notably, the performance of the explicit Euler integrator is poor; the Hessian is indefinite for typical parameter values. Thus, here we use the trace of matrix costs with an implicit integrator. Also note that convexity improves as $\bm{\Sigma}^{f}$ increases, as suggested in the above analysis.

We investigated the ability of the GPs to model contact by verifying 1) that GPs can identify the contact in human-guided contact scenarios, 2) the impact of variation in the contact position, and 3) the impact of environment stiffness on the ability to regress the models. These together allow the direct modeling of environment constraint uncertainty in realistic contact. The hyperparameters in the following were all fit to minimize negative log-likelihood from the same initial values.

When the human guides the robot into contact, the sum of human and environmental forces is measured. We collect human-guided contact and autonomous contact data (i.e. the robot makes contact without human forces), and compare the GPs which are fit to both of them. In Fig. 5a, we see that both models identify the contact location with the hinge mean function, but the human data identifies a lower stiffness and a contact position $1$ cm too high. In the applications here, this error was acceptable, but this is application-specific.

Next, the contact position was varied between trials, with a total variation of $2$ cm over three trials. The result can be seen in Fig. 5b, where the three contact positions can be seen and the higher covariance in the fit GP (green) can be seen compared to the constant contact position (blue).

Finally, contact with environments of various stiffnesses was made, with corresponding stiffness of $[15,45,126]$ N/mm. The resulting quality of the fit can be seen in Fig. 5c, where the hinge function can readily identify the changes in contact position and stiffness.

We first used a contact task to validate safe contact with the environment. In particular, we examined the effects of force chance constraint and well-damped constraint as contact variance or stiffness increases. The robot was taught a task moving from free space into contact, which resulted in GP models as seen in Section 5.3. The contact location could be varied by adding blocks (Fig. 5b), and the environment made stiff by locking out a compliant element (Fig. 5c). For each contact condition–soft, high variance, and stiff–the corresponding model from Section 5.3 was used, and only the MPC parameters relating to the chance constraint ($\overline{f}=12$, $\epsilon=0.5$, chosen by hand tuning) and well-damped constraint ($\xi=1.2$) are changed. The low impedance test condition kept the minimum mass $5$ Kg and damping $500$ Ns/m.

The force trajectories during contact are shown in Fig. 6, averaged over three trials, where variance is indicated by the shaded area. In the soft contact condition, the difference is small as the constraints were not violated. As the variance in the environment increased, the low impedance MPC had a higher contact force, but both constraints reduced this peak force. The chance constraint had more oscillation, but the well-damped constraint was smoother. As the stiffness of the environment increased, the low impepdance MPC had a higher peak contact force, and had unstable contact (i.e., lost contact due to oscillation). The well-damped constraint had more stable contact with fewer oscillations, compared to the chance-constrained MPC.

In the supplementary video, it can be seen that these constraints also affected the behavior approaching contact. Once the chance constraint was activated, the robot pulled back from the contact approach. This was typically accompanied by infeasible optimization problems (i.e., no solution meeting constraints could be found). This problem was empirically more pronounced when using a GP with a hinge function, thus the poor conditioning of stiff contact was identified as a contributing cause. Alternatively, the well-damped constraint paused to allow the damping to increase before making contact. Also, the well-damped constraint was found empirically to have lower computational cost, with an average MPC rate of $22$ compared with $13$ for the chance constraint (Table 2).

Here we present a rail assembly task, where the location of the switch along the rail may vary according to factors not known to the robot, such as existing parts. However, the rail mounting itself is repeatable. This task was taught in three demonstrations from various initial conditions, with varying goal locations along the rail. The desired behavior is that this division of repeatable task DOF and variation can be automatically extracted from data, such that the robot allows the human to more easily manipulate this DOF, i.e., has a lower impedance in the DOF that varies.

The results can be seen in Fig. 8, which shows the linear impedance parameters of mass and damping over space in three validation experiments. It can be seen that the impedance initialized at low values, but as the rail was approached, the impedance in the repeatable DOF was increased substantially, while kept low in the DOF which varied. The supplementary video shows that this results in a human easily intervening to adjust position along the rail. The task could also be achieved autonomously after the demonstrations when a cost was added to keep a positive force reference in the approach direction $\|15-\bm{f}^{r}_{2}\|$.

We also examined a polishing task where the human had to guide a robot-supported polishing spindle along a path. In this case, the human was determining speed along the path, while polishing vibration should not affect the robot’s trajectory. To balance the rejection of polishing force disturbance against the need for the human to easily manipulate the robot, a force disturbance cost was added to the MPC problem. An ergonomic cost is also added, based on the assumed human arm geometry, and tested. The supplementary video shows polishing on a flat surface, corresponding to Fig. 7b, and a round object to show the ability to handle orientation.

As seen in Fig. 9, the different cost function terms affected the rendered impedance along the polishing path. When the frequency-domain disturbance was added, the directionality of the impedance was significantly affected, resulting in a higher impedance cross to the direction of motion, reducing oscillation cross from the direction of motion. This resulted in a reduction of high-frequency velocity, as seen in Fig. 10 and Table 3. Adding a penalty on human joint torques reduced the impedance when farther from the human, as an equivalent force at a greater distance requires more shoulder joint torques due to poor force manipulability. However, the human joint cost did not improve the vibration suppression as significantly as the disturbance cost as seen in Table 3.

This task involved multiple possible goals from the human, with the two goals indicated in Fig. 11(a). While the human objective was not known in advance, the belief was updated online as seen in Fig. 11(b). This update in the belief resulted in different force trajectories, as seen in Fig. 11(c). This allowed the planned robot trajectory to respond online to changes in the human intent, providing 6-DOF assistance to the human.