Ensuring Safe Physical HRI: Integrated MPC and ADRC for Interaction Control

Abstract

This paper proposes a safety-constrained interaction control scheme for robotic manipulators by integrating model predictive control (MPC) and active disturbance rejection control (ADRC). The proposed method is specifically designed for manipulators with complex nonlinear dynamics. To ensure that the control system satisfies safety constraints during human–robot interaction, MPC is incorporated into the impedance control framework to construct a model predictive impedance controller (MPIC). By exploiting the prediction and constraint-handling capabilities of MPC, the controller provides guaranteed safety throughout the interaction process. Meanwhile, ADRC is employed to track the target joint control signals generated by the MPIC, where an extended state observer is utilized to compensate for dynamic modeling errors and nonlinear disturbances within the system, thereby achieving accurate trajectory tracking. The proposed method is validated through both simulation and real-world experiments, achieving high-performance interaction control with safety constraints at a 2 ms control cycle. The controller exhibits active compliant interaction behavior when the interaction stays within the constraint boundaries, while maintaining strict adherence to the safety constraints when the interaction tends to violate them.

1. Introduction

With advancements in actuation technologies, research on rigid robotic manipulators has achieved substantial progress. Leveraging their collaborative capabilities, these manipulators are increasingly deployed in domains such as human–robot interaction [1,2,3], industrial assembly [4,5,6], and industrial manufacturing [7,8,9]. However, achieving efficient and safe contact during interactions with either the environment or human operators remains highly challenging. Human–robot interaction (HRI) has therefore become an important research focus in both academia and industry, aligning with the long-standing goal of developing robotic systems and control strategies capable of interacting safely and effectively with complex environments, particularly with humans. Integrating HRI into industrial settings [10,11,12], while ensuring safe and seamless interactions among humans, robots, and the environment, ultimately poses a fundamental challenge: the design of robust and intelligent control laws.

In robotics, impedance control (IC) is widely employed in human–robot interaction (HRI) [13,14,15,16,17,18,19,20,21,22]. Its fundamental principle is to establish an impedance model between the manipulator and the environment, thereby enhancing HRI safety by regulating the interaction forces and enabling operators to physically guide the robot during operation. However, relying solely on IC for force regulation in HRI exhibits inherent limitations. This approach is inadequate for industrial manufacturing scenarios with stringent safety requirements, as it lacks the ability to predict future system behavior or explicitly handle safety constraints, making it incapable of preventing constraint violations in advance. Such as welding applications featuring strict workspace constraints [23] or tasks performed in a restricted working environment [24]. For these manufacturing scenarios, it is necessary to implement artificial safety constraints during control, including position and velocity constraints [25], to restrict the robot’s operational space.

MPC provides a systematic framework to generate optimal control inputs while explicitly handling predefined constraints and performance objectives. In recent years, MPC has attracted considerable attention and has been widely applied to manipulator control. For instance, the recursive tactile stability observer combined with an MPC-based variable admittance controller in [26] quantitatively evaluates system stability and achieves an optimal balance between stability and flexibility under varying task demands. Similarly, an MPVIC-based framework is presented in [27] to enable compliant interaction between the manipulator and its environment. However, these approaches do not directly embed the impedance model into the design of the MPC cost function. Although [28] formulates the impedance model as a cost function, the method suffers from dimensionality limitations that prevent online computation over longer prediction horizons, and the tracking performance for target joint trajectories is highly dependent on accurate dynamic modeling.

As a robust disturbance rejection methodology, ADRC has been extensively investigated and applied, and has also been extended to robotic manipulator control. By dynamically estimating and compensating for disturbances within the control loop, ADRC substantially reduces the reliance on accurate system modeling. Its core component, the Extended State Observer (ESO), treats the total disturbance as an augmented state variable, enabling the simultaneous estimation of both system states and disturbance states, thereby providing a complete representation of the system’s dynamic characteristics. In [29], a reduced-order ESO suitable for mobile robots is proposed, while [30] introduces a composite observer that integrates the ESO with a Kalman filter. However, these studies have not addressed the use of the ESO for tracking control signals during human–robot interaction. Therefore, the present study aims to extend the application of the ESO to human–robot interaction scenarios.

This paper proposes a control method that integrates model predictive control with active disturbance rejection control, enabling manipulator to achieve trajectory tracking under constraints. By obtaining the current state of the manipulator along with the external forces acting on it, a linear predictive model is employed to estimate the state evolution of the manipulator over a given prediction horizon. The optimal control inputs that satisfy the prescribed safety constraints and objective function are then computed. Specifically, the MPC framework integrates the compliant behavior of impedance control by directly formulating the modeling error of the manipulator’s impedance model as the objective function. Moreover, by constructing the state equations of linear MPC based on the kinematic relationships among Cartesian position, velocity, and acceleration, the proposed method achieves high-frequency control at 500Hz. The method replaces common computed torque control with ADRC, which prevents unstable outputs near constraint boundaries from directly affecting torque output while compensating for modeling errors, thereby ensuring stable operation of the manipulator. The control framework in this paper is illustrated in Figure 1.

2. Control Framework

2.1. Robot Rigid-Body Dynamics

The manipulator used in this study is the Elephant Robotics P3 collaborative robot, which is a 6-dof rigid robotic arm. The dynamic model of the manipulator in joint space can be expressed by

$$M\left(q\right)\ddot{q}+C(q,\dot{q})\dot{q}+G\left(q\right)+F(q,\dot{q})=\tau +{\tau }_{ext}$$

(1)

where q, $\dot{q}$, and $\ddot{q}\in {\mathbb{R}}^6$ represent joint position, velocity, and acceleration, respectively. $M\left(q\right)\in {\mathbb{R}}^{6\times 6}$ denotes the generalized inertia matrix, and $C(q,\dot{q})\in {\mathbb{R}}^{6\times 6}$ is a matrix of the centrifugal and Coriolis torques, and $G\left(q\right)\in {\mathbb{R}}^6$ represents the gravitational torque vector, and $F(q,\dot{q})\in {\mathbb{R}}^6$ is the friction torque vector. $\tau \in {\mathbb{R}}^6$ denotes the control torque vector. Finally, ${\tau }_{ext}\in {\mathbb{R}}^6$ represents the external torque vector.

Therefore, the control law for the computed torque method can be obtained as

$$\tau =\widehat{M}\left(q\right)\eta +\widehat{C}(q,\dot{q})\dot{q}+\widehat{G}\left(q\right)$$

(2)

with

$$\eta =k_p{e}_{R_{{\theta }_1}}+k_d{e}_{R_{{\theta }_2}}+{\widehat{M}}^{-1}\left(q\right)(\widehat{F}(q,\dot{q})-{\tau }_{ext}+d_m)$$

(3)

where $e_{R_{{\theta }_1}},e_{R_{{\theta }_2}}$ are joint position and velocity tracking errors. $\widehat{M}\left(q\right),\widehat{C}(q,\dot{q}),\widehat{G}\left(q\right)$ and $\widehat{F}(q,\dot{q})$ represent the actual identified inertia matrix, centrifugal and Coriolis force matrix, gravity torque vector, and friction torque vector, respectively. $d_m$ denotes the modeling error. $k_p,k_d$ are the proportional coefficient and derivative coefficient, respectively. $\eta$ as the new control input, which calculated by ADRC.

2.2. Impedance Control

The impedance control for the task space is given by

$$M_{\zeta }·({\ddot{p}}_c-{\ddot{p}}_d)+D_{\zeta }·({\dot{p}}_c-{\dot{p}}_d)+K_{\zeta }·(p_c-p_d)=F_e$$

(4)

where $M_{\zeta },D_{\zeta },\mathrm{a}\mathrm{n}\mathrm{d}{K}_{\zeta }\in {\mathbb{R}}^{6\times 6}$ are desired inertia, damping, and stiffness matrices, respectively. Assuming $M_{\zeta }$, $D_{\zeta }$, and $K_{\zeta }$ are positive definite and diagonal. $p_d,{\dot{p}}_d,\mathrm{and}{\ddot{p}}_d\in {\mathbb{R}}^6$ are the target pose in Cartesian space and its first- and second-order differentials. $p_c, {\dot{p}}_c, \mathrm{a}\mathrm{n}\mathrm{d}{\ddot{p}}_c\in {\mathbb{R}}^6$ are the commanded pose in Cartesian space and its first- and second-order differentials, respectively. $F_e$ is the actual force.

Since conventional safety constraints typically do not restrict orientation, this paper focuses on the MPC-based modeling of the translational dimensions of impedance control. The expression corresponding to the translational dimensions of Equation (4) can be expressed by

$$M_{\rho }·({\ddot{\rho }}_c-{\ddot{\rho }}_d)+D_{\rho }·({\dot{\rho }}_c-{\dot{\rho }}_d)+K_{\rho }·({\rho }_c-{\rho }_d)=f_{ext}$$

(5)

where $M_{\rho },D_{\rho },K_{\rho }\in {\mathbb{R}}^{3\times 3}$ are the desired inertia, damping, and stiffness matrices for the translational dimensions. ${\rho }_d,{\dot{\rho }}_d,{\ddot{\rho }}_d\in {\mathbb{R}}^3$ denote the target Cartesian position, velocity, and acceleration, ${\rho }_c,{\dot{\rho }}_c,{\ddot{\rho }}_c\in {\mathbb{R}}^3$ denote the commanded position, velocity, and acceleration adjusted under external forces, $f_{ext}$ represents the first three dimensions of $F_e$.

The workflow of Equation (5) is given by

$$\left\{\begin{array}{c}\begin{array}{cc}\hfill {\ddot{\rho }}_c(t+T_s)& ={\ddot{\rho }}_d(t+T_s)+M_{\rho }^{-1}(f_{ext}\left(t\right)-D_{\rho }({\dot{\rho }}_c\left(t\right)-{\dot{\rho }}_d\left(t\right))\hfill \\ \hfill & -K_{\rho }({\rho }_c\left(t\right)-{\rho }_d\left(t\right)))\hfill \end{array}\hfill \\ {\dot{\rho }}_c(t+T_s)={\dot{\rho }}_c\left(t\right)+{\ddot{\rho }}_c(t+T_s)·T_s\hfill \\ {\rho }_c(t+T_s)={\rho }_c\left(t\right)+{\dot{\rho }}_c(t+T_s)·T_s\hfill \end{array}\right.$$

(6)

where $T_s$ is sampling period.

IC can be represented using a state-space model. To facilitate the establishment of the state-space model, we consider only slowly varying external forces, i.e., ${\dot{f}}_{ext}\approx 0$. Other precise external force models can also be used as alternatives. Based on this assumption, the state equation is established

$$x_{k+1}=A_d{x}_k+B_d{u}_k$$

(7)

with

$$\left\{\begin{array}{c}{x}_k={\left[\begin{array}{c}{\rho }_{c,k}^T{\dot{\rho }}_{c,k}^T{f}_{ext}^T\end{array}\right]}^T\in {\mathbb{R}}^9,u_k={\ddot{\rho }}_{c,k}\in {\mathbb{R}}^3\hfill \\ A_d=\left[\begin{array}{ccc}{I}_3& T_s·I_3& 0\\ 0& I_3& 0\\ 0& 0& I_3\end{array}\right]\in {\mathbb{R}}^{9\times 9}\hfill \\ B_d={\left[\begin{array}{c}0T_s·I_{3}0\end{array}\right]}^T\in {\mathbb{R}}^{9\times 3}\hfill \end{array}\right.$$

(8)

where $I_3\in {\mathbb{R}}^{3\times 3}$ represents an identity matrix, $A_d$ and $B_d$ are obtained using forward Euler discretization.

2.3. Active Disturbance Rejection Control

A practical and effective controller should account for the impact of all potential persistent disturbances and uncertainties. To address this challenge and accommodate unmeasurable and uncertain disturbances in the system [31], this paper employs active disturbance rejection control to replace the common dynamic control law. The control input signal ${\dot{p}}_c$ calculated from Equation (6) is mapped to the joint space

$$\left\{\begin{array}{c}{\dot{q}}_c=J^{-1}\left(q\right){\dot{p}}_c\hfill \\ q_c(t+T_c)=q_c\left(t\right)+{\dot{q}}_c(t+T_c)·T_c\hfill \end{array}\right.$$

(9)

where $T_c$ is control period. ${\dot{q}}_c,q_c\in {\mathbb{R}}^6$ denote joint commanded position and joint commanded velocity, respectively. $J\left(q\right)\in {\mathbb{R}}^{6\times 6}$ denotes the robot Jacobian matrix, which maps the commanded velocity in Cartesian space to the corresponding joint commanded velocity in joint space.

Assume that the Jacobian matrix of the robot relative to the base coordinate system, $J\left(q\right)$ has full rank. The joint commanded position $q_c$ obtained from Equation (9) is input into the tracking differentiator (TD). The form of TD can be expressed by

$$\left\{\begin{array}{c}{g}_h=\mathrm{fhan}(R_{{\theta }_1}\left(t\right)-q_c\left(t\right),R_{{\theta }_2}\left(t\right),r_0,h_0)\hfill \\ R_{{\theta }_1}(t+T_c)=R_{{\theta }_1}\left(t\right)+T_c·R_{{\theta }_2}\left(t\right)\hfill \\ R_{{\theta }_2}(t+T_c)=R_{{\theta }_2}\left(t\right)+T_c·g_h\hfill \end{array}\right.$$

(10)

where fhan function denotes the maximum speed integrating function, $r_0$ is the speed factor, and $h_0$ represents the filtering factor. $R_{{\theta }_1},R_{{\theta }_2}\in {\mathbb{R}}^6$ are the target joint position control signal and target joint velocity control signal obtained through TD filtering. The form of the fhan function is given by

$$g_h=\mathrm{fhan}({\xi }_1,{\xi }_2,r_0,h_0):$$

$$\left\{\begin{array}{c}l=r_0{h}_0,l_0=h_{0}l\hfill \\ u={\xi }_1+h_0{\xi }_2\hfill \\ a_0=\sqrt{l^2+8r_0\left|u\right|}\hfill \\ a=\left\{\begin{array}{cc}{\xi }_2+{\displaystyle \frac{(a_0-l)}{2}}\mathrm{sign}\left(u\right),\hfill & \left|u\right|>l_0\hfill \\ {\xi }_2+{\displaystyle \frac{u}{h_0}},\hfill & \left|u\right|\le l_0\hfill \end{array}\right.\hfill \\ \mathrm{fhan}=\left\{\begin{array}{cc}-r_0\mathrm{sign}\left(a\right),\hfill & \left|a\right|>l\hfill \\ -r_0{\displaystyle \frac{a}{l}},\hfill & \left|a\right|\le l\hfill \end{array}\right.\hfill \end{array}\right.$$

(11)

the filtered target control signals $R_{{\theta }_1}$ and $R_{{\theta }_2}$ are input into the nonlinear state error feedback control law, and combined with the disturbance compensation ${\widehat{x}}_3$ estimated by the ESO to generate the new control input $\eta$ in Equation (2), the output torque for the current cycle is obtained by

$$\left\{\begin{array}{c}{e}_{R_{{\theta }_1}}\left(t\right)=R_{{\theta }_1}\left(t\right)-{\widehat{x}}_1\left(t\right)\hfill \\ e_{R_{{\theta }_2}}\left(t\right)=R_{{\theta }_2}\left(t\right)-{\widehat{x}}_2\left(t\right)\hfill \\ \eta \left(t\right)=k_p{e}_{R_{{\theta }_1}}\left(t\right)+k_d{e}_{R_{{\theta }_2}}\left(t\right)-{\widehat{x}}_3\left(t\right)\hfill \\ \tau \left(t\right)=b^{-1}·[\eta \left(t\right)-f({\widehat{x}}_1,{\widehat{x}}_2)]\hfill \end{array}\right.$$

(12)

where b is the inverse matrix of the inertia matrix $\widehat{M}\left(q\right)$, and $\tau$ is the final output torque of ADRC. In the state error calculation, the estimated joint position ${\widehat{x}}_1\left(t\right)\in {\mathbb{R}}^6$ and estimated joint velocity ${\widehat{x}}_2\left(t\right)\in {\mathbb{R}}^6$ are obtained using an ESO. The function $f(x_1,x_2)=-{\widehat{M}}^{-1}\left(q\right)(\widehat{C}\left(q\right)\dot{q}+\widehat{G}\left(q\right))\in {\mathbb{R}}^6$ represents the feedforward compensation of the model.

The disturbances in the current control system are estimated by the ESO, which can be expressed by

$$\left\{\begin{array}{c}{\epsilon }_{\omega }={\widehat{x}}_1\left(t\right)-x_1\left(t\right)\hfill \\ \begin{array}{c}\hfill {\widehat{x}}_1(t+T_c)={\widehat{x}}_1\left(t\right)+T_c·\left[{\widehat{x}}_2\left(t\right)-{\beta }_1{\epsilon }_{\omega }\right]\end{array}\hfill \\ \begin{array}{cc}\hfill {\widehat{x}}_2(t+T_c)& ={\widehat{x}}_2\left(t\right)+T_c·[{\widehat{x}}_3\left(t\right)+f({\widehat{x}}_1,{\widehat{x}}_2)\hfill \\ \hfill & +b\tau \left(t\right)-{\beta }_2{\epsilon }_{\omega }]\hfill \end{array}\hfill \\ {\widehat{x}}_3(t+T_c)={\widehat{x}}_3\left(t\right)-T_c·{\beta }_3{\epsilon }_{\omega }\hfill \end{array}\right.$$

(13)

where ${\epsilon }_{\omega }\in {\mathbb{R}}^6$ is the position observation error of the ESO, ${\beta }_1,{\beta }_2$, and ${\beta }_3\in {\mathbb{R}}^6$ are the gain parameters of the ESO, and $x_1\in {\mathbb{R}}^6$ is the actual joint position. ${\widehat{x}}_3=-{\widehat{M}}^{-1}\left(q\right)(\widehat{F}(q,\dot{q})-{\tau }_{ext}+d_m)\in {\mathbb{R}}^6$ is the estimate of the total disturbance of the system.

3. Model Predictive Impedance Controller

The core of MPC lies in solving a constrained finite-horizon optimal control problem at each time step. To combine the compliant interaction advantages of impedance control with the constraint-handling capability of MPC while maintaining high control frequency, this paper proposes a model predictive controller based on a three-dimensional impedance model.

3.1. Optimization Problem

In MPC problems, the cost function J should be designed such that its optimal value becomes zero when the system achieves the desired behavior. To satisfy the aforementioned requirements, Equation (5) can be expressed by

$$u_k=u_{d,k}-\Gamma (x_{d,k}-x_k)$$

(14)

with

$$\left\{\begin{array}{c}{u}_{d,k}=\ddot{{\rho }_d}\in {\mathbb{R}}^3\hfill \\ x_{d,k}={\left[\begin{array}{ccc}{\rho }_d^T& {\dot{\rho }}_d^T& 0\end{array}\right]}^T\in {\mathbb{R}}^9\hfill \\ \Gamma =\left[\begin{array}{c}{M}_{\rho }^{-1}{K}_{\rho }{M}_{\rho }^{-1}{D}_{\rho }{M}_{\rho }^{-1}\end{array}\right]\in {\mathbb{R}}^{3\times 9}\hfill \end{array}\right.$$

(15)

The impedance model serves as an ideal interaction model between manipulator and their environment. However, in practical interaction scenarios, the impedance model exhibits inherent errors. To minimize these errors and achieve optimal interaction performance, we directly formulate Equation (14) as the objective function [32]

$$J=\sum _{k=0}^{N_p-1}{(u_k-u_{d,k}+\Gamma (x_{d,k}-x_k))}^{T}R(u_k-u_{d,k}+\Gamma (x_{d,k}-x_k))$$

(16)

It is equivalent to

$$\begin{array}{c}\hfill \underset{u_k}{min}J=\underset{u_k}{min}\sum _{k=0}^{N_p-1}\left||\delta u_k\right|{{|}_{Q_u}^2+\left||\delta x_k\right||}_{Q_x}^2+2\delta x_k^T{Q}_{u,x}\delta u_k\end{array}$$

(17)

where ${∥\delta u_k∥}_{Q_u}^2$ represents the acceleration error term, ${∥\delta u_k∥}_{Q_u}^2={(u_k-u_{d,k})}^T{Q}_u(u_k-u_{d,k})$ and $Q_u=R\in {\mathbb{R}}^{3\times 3}$. Similarly, ${∥\delta x_k∥}_{Q_x}^2$ denotes the state error term, which includes pose error, velocity error, and force error, with ${∥\delta x_k∥}_{Q_x}^2={(x_k-x_{d,k})}^T{Q}_x(x_k-x_{d,k})$, $Q_x={\Gamma }^{T}R\Gamma \in {\mathbb{R}}^{9\times 9}$, and $Q_{u,x}={\Gamma }^{T}R\in {\mathbb{R}}^{9\times 3}$.

The objective function can be transformed into matrix form. First, let us define

$$\left\{\begin{array}{c}\begin{array}{c}{\displaystyle \Delta \mathit{u}={\left[\begin{array}{ccc}\Delta u_0^T& \dots & \Delta u_{N_p-1}^T\end{array}\right]}^T\in {\mathbb{R}}^{3N_p}}\hfill \\ {\displaystyle \mathit{u}={\left[\begin{array}{ccc}{u}_{-1}^T& \dots & u_{N_p-1}^T\end{array}\right]}^T\in {\mathbb{R}}^{3(N_p+1)}}\hfill \\ {\displaystyle \mathit{x}={\left[\begin{array}{ccc}{x}_0^T& \dots & x_{N_p-1}^T\end{array}\right]}^T\in {\mathbb{R}}^{9N_p}}\hfill \\ {\displaystyle \mathit{\mu }={\left[\begin{array}{cccccc}{u}_{d,0}^T& \dots & u_{d,N_p-1}^T& x_{d,0}^T& \dots & x_{d,N_p-1}^T\end{array}\right]}^T\in {\mathbb{R}}^{12N_p}}\hfill \end{array}\hfill \end{array}\right.$$

(18)

through variable substitution, the matrix representation of the objective function can be expressed by

$$\begin{array}{c}\hfill \underset{\Delta \mathit{u}}{min}J=\underset{\Delta \mathit{u}}{min}{(\mathbf{\Psi }\mathit{S})}^T\left[\begin{array}{c}{\mathit{Q}}_{\mathit{u}}{\mathit{Q}}_{\mathit{u},\mathit{x}}^{\mathit{T}}\\ {\mathit{Q}}_{\mathit{u},\mathit{x}}{\mathit{Q}}_{\mathit{x}}\end{array}\right](\mathbf{\Psi }\mathit{S})\end{array}$$

(19)

with

$$\left\{\begin{array}{c}{\displaystyle {\mathit{Q}}_x=\mathrm{diag}(Q_x,\dots ,Q_x)\in {\mathbb{R}}^{9N_p\times 9N_p}}\hfill \\ {\displaystyle {\mathit{Q}}_u=\mathrm{diag}(Q_u,\dots ,Q_u)\in {\mathbb{R}}^{3N_p\times 3N_p}}\hfill \\ {\displaystyle {\mathit{Q}}_{u,x}=\mathrm{diag}(Q_{u,x},\dots ,Q_{u,x})\in {\mathbb{R}}^{9N_p\times 3N_p}}\hfill \\ {\displaystyle \mathit{S}={\left[\begin{array}{cccc}\Delta {\mathit{u}}^T& {\mathit{x}}^T& {\mathit{u}}^T& {\mathit{\mu }}^T\end{array}\right]}^T\in {\mathbb{R}}^{3(9N_p+1)}}\hfill \\ {\displaystyle \mathbf{\Psi }=\left[\begin{array}{cccccc}{I}_{3N_p}& 0& I_{3N_p}& 0& -I_{3N_p}& 0\\ 0& I_{9N_p}& 0& 0& 0& -I_{9N_p}\end{array}\right]\in {\mathbb{R}}^{12N_p\times 3(9N_p+1)}}\hfill \end{array}\right.$$

(20)

We perform a finite-horizon iteration with a prediction horizon $N_p$ for Equation (7), the corresponding expression of which is given by

$$\begin{array}{c}\hfill \mathit{x}={\mathit{M}}_{\mathit{a}}{x}_0+{\mathit{N}}_{\mathit{a}}\mathit{u}\end{array}$$

(21)

with

$$\left\{\begin{array}{c}\begin{array}{cc}\hfill {\mathit{M}}_{\mathit{a}}& =\left[\begin{array}{c}{I}_9\\ A_d\\ A_d^2\\ ⋮\\ A_d^{N_p-1}\end{array}\right]\hfill \\ \hfill {\mathit{N}}_{\mathit{a}}& =\left[\begin{array}{cccccc}0& 0& 0& \dots & 0& 0\\ 0& B_d& 0& \dots & 0& 0\\ 0& A_d{B}_d& B_d& \dots & 0& 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& A_d^{N_p-2}{B}_d& A_d^{N_p-3}{B}_d& \dots & B_d& 0\end{array}\right]\hfill \end{array}\hfill \end{array}\right.$$

(22)

where ${\mathit{N}}_{\mathit{a}}\in {\mathbb{R}}^{9N_p\times 9(N_p+1)}$ is the $N_p$ steps state reachability matrix and ${\mathit{M}}_{\mathit{a}}\in {\mathbb{R}}^{9N_p\times 9}$ is the $N_p$ steps free evolution matrix.

Similarly, the same transformation is applied to $u_k=u_{k-1}+\Delta u_k$, corresponding to the matrix form

$$\begin{array}{c}\hfill \mathit{u}={\mathbf{\Phi }}_{\mathit{a}}\mathbf{\Delta }\mathit{u}+{\mathit{L}}_{\mathit{a}}{u}_{-1}\end{array}$$

(23)

with

$$\left\{\begin{array}{c}\begin{array}{ccc}\hfill {\mathbf{\Phi }}_{\mathit{a}}& =\left[\begin{array}{cccc}0& 0& \dots & 0\\ I_3& 0& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ I_3& I_3& \dots & I_3\end{array}\right]\hfill & \hfill \in {\mathbb{R}}^{3(N_p+1)\times 3N_p}\\ \hfill {\mathit{L}}_{\mathit{a}}& =\left[\begin{array}{c}{I}_3\\ I_3\\ ⋮\\ I_3\end{array}\right]\in {\mathbb{R}}^{3(N_p+1)\times 3}\hfill \end{array}\hfill \end{array}\right.$$

(24)

Substituting Equation (23) into Equation (21) yields

$$\begin{array}{c}\hfill \mathit{x}={\mathit{M}}_{\mathit{a}}{x}_0+{\mathit{N}}_{\mathit{a}}{\mathbf{\Phi }}_{\mathit{a}}\mathbf{\Delta }\mathit{u}+{\mathit{N}}_{\mathit{a}}{\mathit{L}}_{\mathit{a}}{u}_{-1}\end{array}$$

(25)

By combining Equation (23) with Equation (25), Equation (19) can be expressed as

$$\begin{array}{c}\hfill \underset{\Delta \mathit{u}}{min}J=\underset{\Delta \mathit{u}}{min}{\tilde{\mathit{S}}}^T{(\mathbf{\Psi }\mathbf{\Lambda })}^T\mathit{W}(\mathbf{\Psi }\mathbf{\Lambda })\tilde{\mathit{S}}\end{array}$$

(26)

with

$$\left\{{\displaystyle \begin{array}{c}\begin{array}{c}{\displaystyle \mathit{W}=\left[\begin{array}{cc}{\mathit{Q}}_{\mathit{u}}& {\mathit{Q}}_{u,x}^T\\ {\mathit{Q}}_{u,x}& {\mathit{Q}}_x\end{array}\right]}\hfill \end{array}\hfill \\ \tilde{\mathit{S}}={\left[\begin{array}{cccc}\Delta {\mathit{u}}^T& x_0& u_{-1}& {\mathit{\mu }}^T\end{array}\right]}^T\\ \mathbf{\Lambda }=\left[\begin{array}{cccc}{I}_{3N_p}& 0& 0& 0\\ {\mathit{N}}_a{\mathbf{\Phi }}_a& {\mathit{M}}_a& {\mathit{N}}_a{\mathit{L}}_a& 0\\ {\mathbf{\Phi }}_a& 0& {\mathit{L}}_a& 0\\ 0& 0& 0& I_{12N_p}\end{array}\right]\end{array}}\right.$$

(27)

Finally, transform Equation (26) into the corresponding quadratic programming (QP) problem

$$\begin{array}{c}\hfill \underset{\Delta \mathit{u}}{min}J=\underset{\Delta \mathit{u}}{min}\mathbf{\Delta }{\mathit{u}}^T{\mathit{H}}_{\mathbf{1}}\mathbf{\Delta }\mathit{u}+{\mathit{c}}^T\mathbf{\Delta }\mathit{u}\end{array}$$

(28)

with

$$\left\{\begin{array}{c}\begin{array}{c}{\displaystyle \mathbf{\Pi }={\mathbf{\Lambda }}^T{\mathbf{\Psi }}^T\mathit{W}\mathbf{\Psi }\mathbf{\Lambda }}\hfill \\ {\displaystyle \mathbf{\Pi }=\left[\begin{array}{cc}{\mathit{H}}_1& \mathit{P}\\ {\mathit{P}}^T& {\mathit{H}}_2\end{array}\right]}\hfill \\ {\displaystyle \mathit{c}=\mathit{P}·[x_0;u_{-1};\mathit{\mu }]}\hfill \end{array}\hfill \end{array}\right.$$

(29)

Due to the hypotheses of the weight matrices, ${\mathit{H}}_{\mathbf{1}}>0$ and Equation (28) is a convex QP. For Equation (28), when no constraints exist, this convex QP problem can be solved directly using an analytical solution to obtain the optimal solution

$$\begin{array}{cc}\hfill \mathbf{\Delta }{\mathit{u}}^{\ast }& =-{\displaystyle \frac{1}{2}}{\mathit{H}}_{\mathbf{1}}^{-1}·\mathit{c}\hfill \end{array}$$

(30)

3.2. Constraints

In practical manufacturing environments, additional constraints must be considered to ensure HRI safety. Beyond its predictive capabilities, another core advantage of MPC is its inherent ability to incorporate input and output constraints, thereby ensuring system operation within safe boundaries.

In constrained MPC problems, analytical solutions do not exist, necessitating the use of numerical solvers to solve the corresponding QP problem. To achieve this, the constraints should be transformed into standard form.

For the position constraint ${\rho }_c\le {\rho }_{max}$, the corresponding constraint condition is written as

$$\begin{array}{c}\hfill \mathit{x}\le \overline{\mathit{P}}\end{array}$$

(31)

with

$$\left\{\begin{array}{c}{\displaystyle \mathit{x}=[{\rho }_c^T\left(k\right),{\dot{\rho }}_c^T\left(k\right),f_{\mathrm{ext}}^T\left(k\right),{\rho }_c^T(k+1),{\dot{\rho }}_c^T(k+1),f_{\mathrm{ext}}^T(k+1),}\hfill \\ {\displaystyle \dots ,{\rho }_c^T(k+N_p-1),{\dot{\rho }}_c^T(k+N_p-1),f_{\mathrm{ext}}^T(k+N_p-1){]}^T}\hfill \\ \overline{\mathit{P}}={\left[\infty ,\infty ,\infty ,{\rho }_{max}^T,\infty ,\infty ,\dots ,{\rho }_{max}^T,\infty ,\infty \right]}^T\end{array}\right.$$

(32)

Combining Equation (25) yields the standard form of the constraint

$$\begin{array}{c}\hfill {\mathit{N}}_{\mathit{a}}{\mathbf{\Phi }}_{\mathit{a}}\mathbf{\Delta }\mathit{u}\le \overline{\mathit{P}}-{\mathit{M}}_{\mathit{a}}{x}_0-{\mathit{N}}_{\mathit{a}}{\mathit{L}}_{\mathit{a}}{u}_{-1}\end{array}$$

(33)

the process of obtaining the standard form for velocity constraints employs a strategy similar to the aforementioned method.

4. Simulation

This paper constructs a simplified simulated robotic welding scenario in CoppeliaSim V4.3.0 to evaluate the MPIC method through simulation, with the specific simulation setup illustrated in Figure 2.

The control input of the MPIC, $u={\ddot{\rho }}_c\in {\mathbb{R}}^3$, represents the positional acceleration of the control system. The system state variable is defined as $x={\left[{\dot{\rho }}_c^T{\rho }_c^T{f}_{ext}\right]}^T\in {\mathbb{R}}^9$, as shown in Equation (8). The desired control position and control velocity at time step $k+1$ are obtained by

$$\left\{\begin{array}{c}{\dot{\rho }}_c(k+1)={\dot{\rho }}_c\left(k\right)+{\ddot{\rho }}_c(k+1)·T_s\hfill \\ {\rho }_c(k+1)={\rho }_c\left(k\right)+{\dot{\rho }}_c(k+1)·T_s\hfill \end{array}\right.$$

(34)

In this simulation, the control period $T_c$ is equal to the discrete sampling period $T_s$, with $T_c=T_s=0.01$ s. Equations (28) and (33) constitute the MPC optimization problem for this simulation, which is solved online using the C++ convex optimization solver OSQP [33]. The target trajectory in the simulation scenario is designed as a closed semicircle centered at $(-0.0675,0.46)$ m with a radius $r=0.0825$ m. The arc segment is planned using a fifth-order polynomial, while the straight-line segment is planned using S-curve.

In this simulation, the inertial, stiffness, and damping parameters of the MPIC controller are set as $M_{\rho }=[5,5,5]$, $D_{\rho }=[70.7,70.7,31.6]$, and $K_{\rho }=[250,250,50]$, respectively. The optimization parameters for MPC are configured as follows: the prediction horizon $N_p=10$ and the control horizon $N_c=10$. For the constraint conditions in Equation (31), the position constraint is $p_{c,x}\le -0.05$ m, and the velocity constraint is $v_x\le 0.005$ m/s.

Figure 3a presents the simulation results of the welding rod tip trajectory under unconstrained conditions in MPIC. As shown in Figure 3b, the trajectory tracking performance of MPIC under position constraints is demonstrated. The position and velocity values of the control system under velocity constraints, displayed in Figure 4 and Figure 5, indicate the simulation’s effective compliance with velocity constraints.

5. Experiment

To validate the proposed method combining MPIC and ADRC, the Elephant Robotics P3 collaborative robot serves as the experimental platform. The C++ solver OSQP solves the constrained QP problem, with $T_s=2$ ms. The CPU of the controller used in the experiment is Intel(R) Core (TM) i7-6700, with a clock frequency of 3.4 GHz. Additionally, a WHC6L-YB-20A six-axis force sensor from Minxin is mounted at the manipulator’s end-effector to verify force tracking control accuracy, with this sensor capable of sampling at 1 kHz.

To ensure the comparability of experimental results, this paper conducted simple reference trajectory tracking experiments using the Elephant P3 collaborative robot. The target trajectory is a rectangular path measuring $0.15$ m in length and $0.08$ m in width, generated through S-curve planning with a maximum velocity of $v_{\max }=0.01$ m/s. To maintain contact between the end effector and the plane, the planar surface is positioned beneath a rigid plane. The experimental setup is shown in Figure 6.

The experiment consists of four parts. All experimental methods employ ADRC to track the target joint signals. In Section 5.1, comparative experiments between MPIC and IC are performed on the rigid plane without constraints. Section 5.2 presents comparative experiments between MPIC and IC under position constraints on the rigid plane. Section 5.3 demonstrates comparative experiments between MPIC and IC under velocity constraints on the rigid plane. Section 5.4 presents an experiment on safe human–robot interaction under constraints. The parameter settings for the proposed method are as follows: $r_0=20$, $h_0=0.1$, $w_0=90$, ${\beta }_1=3w_0$, ${\beta }_2=3w_0^2$, ${\beta }_3=w_0^3$, $N_p=10$, $N_c=10$, $T_s=2$ ms, $T_c=2$ ms, $M_{\rho }=[5,5,5]$, $D_{\rho }=[100,100,31.6]$, $K_{\rho }=[500,500,50]$.

5.1. Trajectory Tracking Experiment Under Without Constraints

To evaluate the trajectory tracking performance of the proposed MPIC method based on ADRC, a target trajectory tracking experiment is conducted under unconstrained conditions, and the experimental results are presented in Figure 7 and Figure 8.

From Figure 7, the proposed ADRC-based MPIC method achieves precise tracking of the target trajectory, with an average tracking error of ${\overline{e}}_x=0.14$ mm along the x-axis and ${\overline{e}}_y=0.34$ mm along the y-axis. From Figure 8, the proposed method not only ensures accurate trajectory tracking but also provides precise tracking of the target Cartesian velocity. It is worth noting that during interactions between the manipulator and the environment or a human, higher tracking accuracy can be achieved by appropriately increasing the stiffness coefficient $K_{\rho }$, whereas improved compliance can be obtained by reducing $K_{\rho }$ and setting the damping ratio to approximately 0.8–1.

To evaluate the tracking performance of ADRC for the target joint signal, Figure 9 presents a comparison between the actual joint position and the target joint position. Figure 10 compares the system disturbances compensated by the ESO with the actual joint torques.

As shown in Figure 9, the underlying ADRC algorithm maintains effective trajectory tracking performance. The RMSEs are $3.7394\times {10}^{-4}$ rad for joint 1, $2.6978\times {10}^{-4}$ rad for joint 2, and $4.4179\times {10}^{-4}$ rad for joint 3, respectively.

From Figure 10, the variation in the robot’s actual output torque is primarily influenced by the joint torque disturbance compensation based on the ESO. The precise joint trajectory tracking performance is presented in Figure 9. Therefore, these results indicate that the ESO-based joint torque disturbance compensation is highly accurate, particularly during joint reversal instances. For example, at $t=10$ s and $t=40$ s, the torque compensation for the reversals of joint 1 is both rapid and precise. It should be noted that the torque disturbance compensation for joints 2 and 3 in the Figure exhibits certain discrepancies compared with the actual torques. This is mainly because joints 2 and 3 are affected by gravitational forces, while the torque disturbance compensation does not include gravity compensation. Instead, the gravitational term is compensated in a feedforward manner through $\widehat{G}\left(q\right)$.

5.2. Trajectory Tracking Experiment Under Position Constraint

To verify the capability of the proposed ADRC-based MPIC method to satisfy the prescribed constraint boundaries, the constraint condition is set as $p_{c,x}\le 0.01$ m. The position tracking results under the given position constraint $p_{c,x}\le 0.01$ m are presented in Figure 11. The velocity tracking results under the given position constraint $p_{c,x}\le 0.01$ m are presented in Figure 12.

From Figure 11, the proposed ADRC-based MPIC method effectively complies with the prescribed safety constraints. When the end-effector load of the manipulator exceeds the specified safety boundary, the MPIC responds rapidly to restore it within the constraint limits. Meanwhile, under the given constraint, the controller determines the optimal control signal that minimizes the trajectory tracking error at the current time step, and the ADRC ensures precise tracking of the control signal.

From Figure 12, when the end-effector load approaches the position constraint boundary $p_{c,x}\le 0.01$ m, the MPIC rapidly reduces ${\dot{p}}_{c,x}$ to near zero, ensuring that the control system complies with the prescribed position constraint.

For the optimal target control signal generated by the MPIC, the ADRC is employed to track the corresponding target trajectory. The resulting joint trajectory tracking performance is illustrated in Figure 13, while Figure 14 presents a comparison between the actual torque and the torque disturbance compensation estimated by the ESO.

From Figure 13 and Figure 14, during t = 25–30 s, the ESO rapidly provides feedforward compensation for the control torque of joint 1, enabling the joint to accurately track the target position and promptly halt its motion in this phase, thereby effectively preventing the manipulator from violating the prescribed position constraint.

5.3. Human–Robot Interaction Experiment Under Position Constraint

To verify the compliance performance of the ADRC-based MPIC method with the prescribed safety position boundary under HRI conditions, we conduct HRI experiments at the constraint boundary, where external forces that violate the constraint are applied. The experimental results are presented in Figure 15 and Figure 16.

When the manipulator approaches the position constraint boundary $p_{c,x}\le 0.01$ m, an external force is applied as illustrated in Figure 16, and the resulting motion trajectory of the manipulator under this force is presented in Figure 15. From Figure 15 and Figure 16, when the applied external force satisfies the position constraint $f_x<0$, the end-effector exhibits desirable active compliance. Conversely, when the applied force violates the position constraint $f_x>0$, the end-effector no longer maintains active compliance but is instead strictly constrained by the MPIC near $p_{c,x}=0.01$ m.

Due to the additional external force applied to the end-effector load, the verification of ADRC’s capability to compensate for sudden external force disturbances is carried out. Figure 17 illustrates the joint trajectory tracking performance under human–robot interaction, and Figure 18 shows the corresponding comparison between the actual joint torques and the torque disturbance compensation achieved by the ESO.

The green region in Figure 17 illustrates that the target trajectories of each joint fluctuate under the influence of external forces. However, the ESO effectively compensates for the torque disturbances caused by these external forces, enabling all joints to accurately track their desired trajectories. The red region in Figure 18 shows that, under the influence of external forces, the ESO rapidly updates the current disturbance compensation, enabling the joint torques to respond quickly and thus achieve precise tracking of the target trajectory.

5.4. Trajectory Tracking Experiment Under Velocity Constraint

To evaluate the effectiveness of the proposed ADRC-based MPIC method in ensuring compliance with the velocity safety constraint, the velocity limit is set to ${\dot{p}}_{c,x}\le 0.005$ m/s. The corresponding trajectory tracking performance of the manipulator end-effector under the velocity constraint is illustrated in Figure 19 and Figure 20.

From Figure 19 and Figure 20, the original reference trajectory violates the safety constraint because its maximum reference velocity exceeds the specified safe velocity. To comply with the imposed velocity constraint, the MPIC restricts the maximum velocity to remain close to the safety limit, causing the trajectory of the end-effector load to deviate from the reference trajectory. The experimental results verify that the proposed ADRC-based MPIC method can strictly satisfy the prescribed velocity constraint while achieving the best possible tracking performance under constrained conditions. This capability provides a useful reference for interactive applications that require strict velocity safety assurance.

6. Discussion

The proposed MPIC method integrated with ADRC, as demonstrated by the results of the designed experiments, exhibits excellent active compliance within the constrained range and can effectively avoid contact force behaviors that violate constraints. However, this paper still has several limitations. First, due to the requirements of MPC modeling, external force variations are regarded as slowly changing contact forces in this work. Although feedback correction is implemented using high-real-time measured forces from a force sensor in each control cycle, limitations remain for HRI scenarios involving sudden contact forces. In future research, if a more accurate external force model can be developed, it can be incorporated into the state equation established in this paper to improve modeling accuracy and system robustness. Second, the feasibility of the MPC optimization problem constructed in this paper depends on the length of the prediction horizon $N_p$. An excessively short $N_p$ may lead to infeasible solutions when solving the constrained MPC optimization problem, while an overly long $N_p$ will prevent the MPC problem from being solved in high-real-time scenarios. Subsequent work can adopt a hierarchical control strategy to improve the proposed control framework, reducing the sampling frequency of MPIC to enable the solution of optimization problems with longer $N_p$ and enhance its predictive capability. Finally, Appendix A provides proofs of stability and convergence for ADRC, but the stability and convergence of MPIC cannot yet be verified. Future research may consider using tank-based methods to ensure the stability of the system [27].

7. Conclusions

The proposed control method retains the characteristics of traditional impedance control while incorporating the capability of MPC to handle multivariable and multi-constraint systems. Additionally, ADRC compensates for modeling errors and external disturbances through disturbance rejection. Comparative experiments conducted on the Elephant P3 robot demonstrate that the proposed control method strictly adheres to position and velocity constraints while achieving optimal trajectory tracking under these constraints. These findings provide new insights for numerous potential applications, particularly in the field of safe human–robot interaction. However, the current work has some limitations. For instance, the adaptive control laws should be introduced to handle interactions in complex external environments, ensuring real-time updates of impedance parameters during control period. This also raises the question of how to maintain high-frequency control performance under varying Hessian matrix conditions.