Coupled Force–Position Control for Dynamic Contact Force Tracking in Uncertain Environment

Abstract

Both the position and force control of robots are needed in industrial manufacturing, such as in assembly and grinding, etc. In this paper, we concentrate on two issues. One is the system oscillation in traditional hybrid force–position control (HFPC) during switching between force and position control because the diagonal elements in the selection matrix are either 0 or 1. Another issue is the poor force-tracking performance of conventional impedance control, which depends on accurate environmental models. To address these issues, a coupled force–position control (CFPC) method is presented in this paper by combining the proposed adaptive impedance control method with a modified HFPC method. The selection matrix $\mathit{S}$ of HFPC is replaced with a weighted matrix ${\mathit{S}}_{\mathrm{w}}$. A weighted matrix regulator is designed to realize smooth switching between position and force control by adjusting the matrix weights in real time, and an adaptive impedance control algorithm is proposed to improve the force-tracking performance in complex environments. To verify the feasibility of the CFPC method proposed in this paper, simulations and physical experiments were conducted. The results show that the CFPC method has the advantages of a better force-tracking performance and a smoother switching between position and force control compared to the traditional HFPC method. A grinding experiment was conducted to further compare the performances of the HFPC and CFPC methods. The roughness values of the ground plates were 0.059 μm for the HFPC method and 0.031 μm for the proposed CFPC method, which demonstrates that the proposed CFPC method has a better performance.

1. Introduction

Force and position are two typical control objects in robotic control problems. In many robotic applications, such as peg-in-hole assembly, grinding, and polishing, etc., it is necessary to control both force and position. The robot generally needs to switch frequently between a free space and a contact-manipulating space, where the robot should be compliant. This is an essential problem when designing a compliance controller that has the following advantages: it not only implements smooth changes between the force and position control modes without system oscillation and contact force overshoot but also performs excellently in terms of force tracking in complicated environment.

There are two major compliance control methods, which are impedance control and hybrid force–position control (HFPC) [1,2]. Impedance control, introduced by Hogan [3], can be used for force tracking by adjusting the dynamic relation between the motion and the contact force. This indirect force control method is widely used owing to its easy implementation and robustness [4,5,6,7]. For instance, Yang et al. [8] employed an observer in the robot joint space to estimate the interaction torque of the robot and used impedance control to adjust the position and orientation of the manipulator at the interaction points. However, traditional impedance control depends on an accurate environmental model. The force-tracking performance is poor in an unstructured environment. To solve this problem, some researchers proposed adaptive impedance control methods by combining adaptive control with impedance control. Li et al. [9] proposed an adaptive impedance control algorithm and established a linear quadratic regulator using reinforcement learning to achieve minimum error tracking. Ikeura et al. [10] and Lee et al. [11] presented a variable impedance control method to deal with force-tracking problems in varying environment. Sheng et al. [12] presented an adaptive impedance control method for mirror milling. Mei et al. [13] proposed an adaptive unified impedance and admittance control method to solve the problem of interactions between the robot and the environment. Although the force-tracking performance improved, overshoot in the contact force is always ignored, which may damage the workpiece. Moreover, the impedance control couples force and position, force control always affects positioning accuracy [14], so impedance control cannot be applied for accurate position tracking in a free space.

Unlike impedance control, HFPC divides the robot task space into two orthogonal subspaces and controls the force and position independently [15,16,17]. Ravandi et al. [18] proposed an HFPC method that combines fuzzy logic with traditional sliding mode control to obtain precise force tracking in uncertain environments. Xie et al. [19] presented an HFPC method to achieve motion tracking and force controlling. A hybrid force-position controller based on fuzzy PID was designed to improve the quality of aviation blade grinding [20]. Anh et al. [21] proposed a hybrid force–position control algorithm based on a neural network to improve the performance of the traditional HFPC method. Although this method is robust, the diagonal elements in the selection matrix of a traditional HFPC method only have two states, i.e., 0 and 1. Consequently, when a manipulator switches from position control to force control, oscillation occurs. Furthermore, it is always difficult to judge when to switch the control mode precisely because environmental information is lacking.

Aiming to address the issues mentioned above, our goal is to design a controller that has the advantages of autonomous smooth switching between the force and position control modes, and also having good performance in terms of force tracking in complicated environments. We make some efforts, as follows:

• The selection matrix $\mathit{S}$ of HFPC is replaced by weighted matrix ${\mathit{S}}_{\mathrm{w}}$, and a weight matrix regulator is designed to achieve automatic smooth switching between position and force control by adjusting the matrix weights in real time according to contact force feedback.
• An adaptive impedance controller is proposed to implement force tracking in complicated environment, and its stability is also analyzed.
• Combining the proposed adaptive impedance controller with the modified HFPC method, we present a coupled force–position control (CFPC) method in this paper. This method has the merits of an automatic smooth switching between the free space and the interaction manipulating space without contact force overshoot and system oscillation.

The remainder of this paper is organized as follows. In Section 2, a Kalman filter is used to estimate the contact force between the manipulator end-effector and the environment. The design of the coupled force–position controller is presented. The adaptive impedance controller is designed, and its stability is also analyzed. In Section 3, simulations and experiments are conducted to demonstrate the performance and applicability of the proposed CFPC method. Finally, a conclusion is drawn in Section 4.

2. Proposal of the Coupled Force–Position Controller

2.1. Contact Force Estimation

The contact force model comprises a robot model and an environmental model. The robot model is represented by a second-order mass-spring-damper model, and the environment is represented as a rigid model, as shown in Figure 1.

Figure 1. Contact force model for estimation.

The implementation of the proposed CFPC method requires measuring the contact force between the manipulator end-effector and the environment. To obtain an accurate contact force from noisy measurements (the gravity of the tool has been compensated via the gravity-compensation algorithm; assuming the manipulator moves slowly, the influence of the inertia of the tool can be ignored), the Kalman filter, which is a widely applied Bayesian estimation method, is used to estimate the contact force. The state equations are given by

$$\dot{X}(t)=A_{0}X\left(t\right),$$

(1)

$$Y(t)=H_{0}X\left(t\right),$$

(2)

where $X$ and $Y$ denote the system state vector and measurement vector, respectively; $X={\left[\begin{array}{cccc}{F}_s^T& T_s^T& {\dot{F}}_s^T& {\dot{T}}_s^T\end{array}\right]}^T$; $Y={\left[\begin{array}{cc}{F}_s^T& T_s^T\end{array}\right]}^T$; $F_s$ and $T_s$ represent force and torque measured by the six-axis force–torque sensor, respectively; and $A_0$ and $H_0$ are expressed as follows:

$$A_0=\left[\begin{array}{cc}{0}_{6\times 6}& I_{6\times 6}\\ 0_{6\times 6}& 0_{6\times 6}\end{array}\right],$$

$$H_0=\left[\begin{array}{cc}{I}_{6\times 6}& 0_{6\times 6}\end{array}\right].$$

The system and the measurement models can be expressed as follows:

$$\dot{X}\left(t\right)=A_{0}X\left(t\right)+w,$$

(3)

$$Y(t)=H_{0}X\left(t\right)+v,$$

(4)

where $w$ and $v$ denote process noise and measurement noise, respectively. Equations (3) and (4) can be discretized as follows:

$$X_k=A{X}_{k-1}+w_{k-1},$$

(5)

$$Y_k=H{X}_k+v_k,$$

(6)

where

$$A=\left[\begin{array}{cc}{I}_{6\times 6}& I_{6\times 6}\\ 0_{6\times 6}& I_{6\times 6}\end{array}\right],$$

$H=H_0$, $X_k$ represents the force signal at the k-th moment, and the iterative process of the Kalman filter is determined by the following Equations [22]:

$${\widehat{x}}_k=A{x}_{k-1},$$

(7)

$${\widehat{P}}_k=A{P}_{k-1}{A}^T+Q,$$

(8)

$$\phi ={\widehat{P}}_k{H}^T{\left(H{\widehat{P}}_k{H}^T+R\right)}^{-1},$$

(9)

$$x_k={\widehat{x}}_{k-1}+\phi \left(Y_k-H{\widehat{x}}_k\right),$$

(10)

$$P_k=\left(I-KH\right){\widehat{P}}_k,$$

(11)

where $Q$, $\phi$, $R$, $P$, ${\widehat{x}}_k$, and ${\widehat{P}}_k$ denote the process noise, Kalman gain, measurement noise, covariance, predictive system sate, and predictive covariance, respectively.

2.2. CFPC Modeling

The contact process between the robot and the environment can be divided into two phases: the first phase is the robot moving in a free space, and the second phase is robot’s contact with the environment. In such a contact process, the robot needs to switch from position control to force control correspondingly. The traditional HFPC method switches between force and position control using a selection matrix $S$ in the controller. However, each diagonal element in the selection matrix of the traditional HFPC method is either 0 or 1, the system oscillates when switching from one control mode to the other, and it is always difficult to judge when to switch the control mode precisely in complex environments. To address this problem, we replaced the selection matrix $S$ with the weighted matrix $S_{\mathrm{w}}$. The control block diagram of the proposed CFPC is shown in Figure 2. $D_r$ and $F_d$ stand for the desired trajectory and desired force, and $I$ denotes the identity matrix. We also designed a weight matrix regulator to realize automatic smooth switching between position and force control by adjusting the matrix weights in real time according to contact force feedback. The matrix weights are adjusted in Algorithm 1. $f_i$ is the contact force between the end-effector of the manipulator and the environment, $f_{\lim }$ is the contact force threshold, $\lambda$ is the exponent that determines the transition curve of the weights and is set according to different requirements of force sensitivity, and $S_{wi}$ is the diagonal element of $S_{\mathrm{w}}$.

Algorithm 1. Weight Adjustment

Input  $f_i$, $f_{\lim }$, $\lambda$ if $\left|f_i\right|\ge f_{\lim }$, $s_{wi}=0$ else if $0<\left|f_i\right|<f_{\lim }$, $s_{wi}=\left|\frac{1}{f_{\lim }^{\lambda }}{\left(\left|f_i-f_{\lim }\right|\right)}^{\lambda }\right|$ else $s_{wi}=1$ output $s_{wi}$

Figure 2. Control block diagram of the proposed CFPC method.

2.3. Force Controller Design

Generally, the commercial manipulator performs position tracking excellently, so we merely focus on the force controller design. Impedance control has the advantage of an easy implementation, but its force-tracking performance is poor in complex environments, since it depends on an accurate environmental model [23]. To overcome this limitation, we propose an adaptive impedance control algorithm to improve the performance of force tracking.

Figure 3 shows the block diagram of the proposed adaptive impedance control method. The control algorithm can be expressed as folles:

$$M\left({\ddot{D}}_c-{\ddot{D}}_r\right)+B\left({\dot{D}}_c-{\dot{D}}_r\right)+K\left(D_c-D_r\right)=F_e-F_d,$$

(12)

where $D_c$, $F_e$, $M$, $B$, and $K$ denote trajectory sent to the robot, contact force between the end-effector and environment, mass, damping, and stiffness, respectively.

Figure 3. Adaptive impedance controller.

Without loss of generality, we study the one-dimensional case. Equation (12) can be rewritten as follows:

$$m\left({\ddot{d}}_c-{\ddot{d}}_r\right)+b\left({\dot{d}}_c-{\dot{d}}_r\right)+k\left(d_c-d_r\right)=f_e-f_d.$$

(13)

As the accurate environmental stiffness $k_e$ is unknown, reference trajectory $d_r$ is difficult to obtain. Therefore, the trajectory can be replaced with the environmental location $d_e$ [24], and Equation (13) can be expressed as follows:

$$m\left({\ddot{d}}_c-{\ddot{d}}_e\right)+b\left({\dot{d}}_c-{\dot{d}}_e\right)+k\left(d_c-d_e\right)=f_e-f_d.$$

(14)

By letting position perturbation $e=d_c-d_e$, Equation (14) becomes the following:

$$m\ddot{e}+b\dot{e}+ke=f_e-f_d.$$

(15)

Because an error exists between the actual and the measured environmental locations, estimation should be performed. Let ${d^{\prime }}_e$ denote the estimation of the environmental location and $\mathsf{\Delta }{d}_e$ stand for the estimated error of the environmental location, which can be expressed as $\mathsf{\Delta }{d}_e=d_e-{d^{\prime }}_e$. Define $e^{\prime }=e+\mathsf{\Delta }{d}_e$; then, Equation (15) can be rewritten as follows:

$$m{\ddot{e}}^{\prime }+b{\dot{e}}^{\prime }+k{e}^{\prime }=f_e-f_d=m\left(\ddot{e}+\mathsf{\Delta }{\ddot{d}}_e\right)+b\left(\dot{e}+\mathsf{\Delta }{\dot{d}}_e\right)+k\left(e+\mathsf{\Delta }{d}_e\right),$$

(16)

where $\mathsf{\Delta }d$ and $d_e$ are time-varying parameters, indicating the persistent force-tracking error.

We propose the following adaptive impedance control algorithm:

$$m{\ddot{e}}^{\prime }\left(t\right)+b\left({\dot{e}}^{\prime }\left(t\right)+Q_b\left(t\right)\right)+k\left(e^{\prime }\left(t\right)+Q_k\left(t\right)\right)=f_e\left(t\right)-f_d\left(t\right),$$

(17)

where

$$Q_b\left(t\right)=Q_b\left(t-\alpha \right)+\beta \frac{f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)}{b},$$

(18)

$$Q_k\left(t\right)=Q_k\left(t-\alpha \right)+\beta \frac{f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)}{k},$$

(19)

with $Q_k\left(t\right)$ and $Q_b\left(t\right)$ being adjusted according to the force error. $\alpha$ and $\beta$ denote the sampling period and update rate of the system, respectively.

2.4. Stability Analysis of the Force Controller

In this section, we analyze the stability of the force controller we designed in this paper. To analyze the stability of the force controller, we first substitute (18) and (19) into (17) to obtain

$$\begin{array}{l}m{\ddot{e}}^{\prime }\left(t\right)+b{\dot{e}}^{\prime }\left(t\right)+k{e}^{\prime }\left(t\right)+b{Q}_b\left(t-\alpha \right)+{\beta }_b[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]\\ +k{Q}_k\left(t-\alpha \right)+{\beta }_k[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]=f_e\left(t\right)-f_d\left(t\right)=\\ m\left(\ddot{e}\left(t\right)+\mathsf{\Delta }{\ddot{d}}_e\left(t\right)\right)+b\left(\dot{e}\left(t\right)+\mathsf{\Delta }{\dot{d}}_e\left(t\right)\right)+k\left(e\left(t\right)+\mathsf{\Delta }{d}_e\left(t\right)\right)+\\ b{Q}_b\left(t-\alpha \right)+{\beta }_b[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]+k{Q}_k\left(t-\alpha \right)+\\ {\beta }_k\left[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)\right]\end{array}$$

(20)

It follows that

$$\begin{array}{l}m\ddot{e}\left(t\right)+b\dot{e}\left(t\right)+ke\left(t\right)-[f_e\left(t\right)-f_d\left(t\right)]+\\ b{Q}_b\left(t-\alpha \right)+{\beta }_b[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]\\ +k{Q}_k\left(t-\alpha \right)+{\beta }_k[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]\\ =-m\mathsf{\Delta }{\ddot{d}}_e\left(t\right)-b\mathsf{\Delta }{\dot{d}}_e\left(t\right)-k\mathsf{\Delta }{d}_e\left(t\right)\end{array}$$

(21)

According to the environmental model $f_e=k_e\left(d_e-d_c\right)=-k_{e}e$, we can obtain

$$e=-\frac{f_e}{k_e}\hspace{1em}\hspace{1em}\dot{e}=-\frac{{\dot{f}}_e}{k_e}\hspace{1em}\hspace{1em}\ddot{e}=-\frac{{\ddot{f}}_e}{k_e}.$$

(22)

where $k_e$ denotes environmental stiffness.

Upon substituting (22) into (21), we obtain

$$\begin{array}{l}-m{\ddot{f}}_e\left(t\right)-b{\dot{f}}_e\left(t\right)-k{f}_e\left(t\right)-k_e[f_e\left(t\right)-f_d\left(t\right)]\\ +b{k}_e{Q}_b\left(t-\alpha \right)+k_e{\beta }_b[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]\\ +k{k}_e{Q}_k\left(t-\alpha \right)+k_e{\beta }_k[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]\\ =-m{k}_e\mathsf{\Delta }{\ddot{d}}_e\left(t\right)-b{k}_e\mathsf{\Delta }{\dot{d}}_e\left(t\right)-k{k}_e\mathsf{\Delta }{d}_e\left(t\right)\end{array}$$

(23)

It follows that

$$\begin{array}{l}m{\ddot{f}}_d\left(t\right)-m{\ddot{f}}_e\left(t\right)+b{\dot{f}}_d\left(t\right)-b{\dot{f}}_e\left(t\right)+k{f}_d\left(t\right)-k{f}_e\left(t\right)-\\ k_e[f_e\left(t\right)-f_d\left(t\right)]+b{k}_e{Q}_b\left(t-\alpha \right)+k_e{\beta }_b[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]\\ +k{k}_e{Q}_k\left(t-\alpha \right)+k_e{\beta }_k[f_d\left(t-\alpha \right)-f_e\left(t-\alpha \right)]\\ =m{\ddot{f}}_d\left(t\right)-m{k}_e\mathsf{\Delta }{\ddot{d}}_e\left(t\right)+b{\dot{f}}_d\left(t\right)-b{k}_e\mathsf{\Delta }{\dot{d}}_e\left(t\right)+k{f}_d\left(t\right)-k{k}_e\mathsf{\Delta }{d}_e\left(t\right)\end{array}$$

(24)

Let ${f^{\prime }}_e\left(t\right)=k_e\mathsf{\Delta }{d}_e\left(t\right)$, $o\left(t\right)=f_d\left(t\right)-f_e\left(t\right)$, and $p\left(t\right)=f_d\left(t\right)-{f^{\prime }}_e$. Equation (24) can be rewritten as

$$\begin{array}{l}m\ddot{o}\left(t\right)+b\dot{o}\left(t\right)+ko\left(t\right)+k_{e}o\left(t\right)+b{k}_e{Q}_b\left(t-a\right)\\ +k_{e}bo\left(t-a\right)+k{k}_e{Q}_k\left(t-a\right)+k_{e}bo\left(t-a\right)\\ =m\ddot{p}\left(t\right)+b\dot{p}\left(t\right)+kp\left(t\right)\end{array}$$

(25)

and n elements of $Q_b\left(t\right)$ and $Q_k\left(t\right)$ series can be discretized as [24],

$$\begin{array}{l}b{Q}_b\left(t-\alpha \right)=b{Q}_b\left(t-\left(n+1\right)\alpha \right)\\ +\beta o\left(t-\left(n+1\right)\alpha \right)+\cdots +\beta o\left(t-2\alpha \right)\end{array}$$

(26)

$$\begin{array}{l}k{Q}_k\left(t-\alpha \right)=k{Q}_k\left(t-\left(n+1\right)\alpha \right)\\ +\beta o\left(t-\left(n+1\right)\alpha \right)+\cdots +\beta o\left(t-2\alpha \right)\end{array}$$

(27)

Using the common initial value of zero for $Q_b\left(t-\left(n+1\right)\alpha \right)$ and $Q_k\left(t-\left(n+1\right)\alpha \right)$, Equation (25) becomes

$$\begin{array}{l}m\ddot{o}\left(t\right)+b\dot{o}\left(t\right)+ko\left(t\right)+k_{e}o\left(t\right)\\ +2k_e\beta \left(o\left(t-\left(n+1\right)\alpha \right)+\cdots +o\left(t-\alpha \right)\right)\\ =m\ddot{p}\left(t\right)+b\dot{p}\left(t\right)+kp\left(t\right)\end{array}$$

(28)

in which the Laplace transformation gives

$$\frac{o\left(s\right)}{p\left(s\right)}=\frac{m{s}^2+bs+k}{m{s}^2+bs+\left(k+k_e\right)+2k_e\beta \left(e^{-(n+1)\alpha s}+\cdots +e^{-\alpha s}\right)}.$$

(29)

From (29), we can obtain the stability condition of the system:

$$m{s}^2+bs+\left(k+k_e\right)+2k_e\beta \left(e^{-\left(n+1\right)\alpha s}+\cdots +e^{-\alpha s}\right)=0.$$

(30)

Assuming that $n$ is large and that $\alpha$ is small, such that ${\displaystyle \sum _{n=1}^{\infty }{e}^{-\alpha sn}}=\frac{e^{-\alpha s}}{1-e^{-\alpha s}}$, $e^{-\alpha s}\approx 1-\alpha s$. Equation (30) can be rewritten as

$$\alpha m{s}^3+\alpha b{s}^2+\alpha \left(k+k_e\right)s+\left(\alpha (k+k_e)-2k_e\beta \right)s+2k_e\beta =0.$$

(31)

According to Routh’s stability criterion, we obtain

$$\{\begin{array}{c}2k_e\beta >0\\ \alpha \left(k+k_e\right)-2k_e\beta >0\\ \frac{{\alpha }^{2}b\left(k+k_e(1-2\beta )\right)-2\alpha m{k}_e\beta }{\alpha b}>0\end{array},$$

(32)

and the stable condition of the system is given as follows:

$$0<\beta <\frac{\alpha b\left(k+k_e\right)}{2k_e\left(\alpha b+m\right)}.$$

(33)

The steady-state error of the stable system can be obtained by Laplace transform:

$$\begin{array}{l}\underset{t\to \infty }{lim}\left(f_d\left(t\right)-f_e\left(t\right)\right)=\underset{t\to \infty }{lim}o\left(t\right)=\underset{s\to 0}{lim}so\left(s\right)\\ =\frac{m{s}^2+bs+k}{m{s}^2+bs+k+k_e+2k_e\beta \left(\frac{1-\alpha s}{\alpha s}\right)}p\left(s\right)\end{array}$$

(34)

Assuming that the input of the system is a step function, $p\left(s\right)=\frac{1}{s}$; when $t\to \infty$, we have $\underset{t\to \infty }{lim}\left(f_d\left(t\right)-f_e\left(t\right)\right)=0$. Therefore, the contact force $f_e$ between the end-effector and the environmental approaches the desired force $f_d$ as $t\to \infty$. This fact shows that the adaptive impedance controller designed in this paper is stable. In Section 3, We also confirm this through simulations and experiments, in which a complex input function in the system provides a tracking error close to zero.

3. Simulations and Experiments

3.1. Simulations

To verify the effectiveness and performance of the proposed controller, we used a Python implementation to simulate CFPC. We compared the force-tracking performance on a plane, a slope, and a complex surface using HFPC and CFPC. The simulation parameters are listed in Table 1, and the simulation results are shown in Figure 4, Figure 5 and Figure 6.

Table 1. Simulation parameters.

Parameter	m	$b$	$k$	$k_e$	$\lambda$	$\alpha$	$\beta$	$R$
Value	0.5	120	2000	5000	1.2	0.005	0.35	0.001

Figure 4. Simulations on plane surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Figure 5. Simulations on sloped surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Figure 6. Simulations on complex surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Figure 4a, Figure 5a, and Figure 6a show the variable force tracking of HFPC on the plane, sloped, and complex surfaces, respectively. While switching between position and force control, the system oscillates and an overshoot occurs for large tracking forces. Figure 4b, Figure 5b, and Figure 6b show the variable force tracking of the proposed CFPC method on the plane, sloped, and complex surfaces. The contact force approaches the desired force smoothly without overshoot. The results of the position tracking are shown in Figure 4c, Figure 5c, and Figure 6c. The position-tracking error results in a large force-tracking error in the initial phase when using HFPC. The weights of position control and force control are shown in Figure 4d, Figure 5d, and Figure 6d, verifying the smooth change.

3.2. Experiments

The proposed CFPC method was also verified through physical experiments. The experimental platform mainly comprises a computer (Intel Core i7: Intel Corporation, Ubuntu 18.04, Python 2.7), robot controller, robot body (Universal Robots UR5: Universal Robots A/S Energivej 25 DK-5260, Odense, Danmark, maximum load: 5 kg, communication frequency: 125 Hz), six-axis force–torque sensor (Universal Robots NRS-6050-D80, maximum force/torque: ±500 N/±10 Nm, sampling rate: <1000 Hz, force/torque resolution: 0.015 N/0.312 × 10⁻³ Nm), and signal converter. The details of the experimental platform are shown in Figure 7.

Figure 7. Schematic of the experimental platform.

To demonstrate the feasibility of the proposed CFPC method, we compared the performances of the HFPC and CFPC methods. We conducted constant force-tracking and variable force-tracking experiments on sloped, curved, and complicated surfaces, as shown in Figure 8a,b and Figure 9. The results are shown in the Video S1. The results of force-tracking experiments on sloped, curved surfaces are shown in Figure 10, Figure 11, Figure 12 and Figure 13. The constant force and variable force-tracking experiments on sloped and curved surfaces in Figure 10a, Figure 11a, Figure 12a, and Figure 13a show that the HFPC method demonstrates a large overshoot of contact forces when switching between position and force control and that system oscillation also occurs. In contrast, Figure 10b, Figure 11b, Figure 12b, and Figure 13b show that the proposed CFPC method can realize smooth switching between position and force control. Figure 10d, Figure 11d, Figure 12d, and Figure 13d show the smooth transition between position and force control. The results of position tracking are shown in Figure 10c, Figure 11c, Figure 12c, and Figure 13c.

Figure 8. Experimental platform when executing (a) force tracking on a sloped surface and (b) force tracking on curved surface, and (c) when grinding.

Figure 9. Experimental platform when executing force tracking on a complex surface.

Figure 10. Constant force-tracking experiment on a sloped surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Figure 11. Variable force-tracking experiment on a sloped surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Figure 12. Constant force-tracking experiment on curved surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Figure 13. Variable force-tracking experiment on a curved surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

We also conducted this experiment in a complicated environment where the controller needs to switch frequently between the position and force control. This complicated environment includes a sloped, a curved surface, and a step surface. The experiment results are shown in Figure 14. The overshoot of contact forces and system oscillation are obvious when using HFPC, as shown in Figure 14a, because the traditional HFPC depends on accurate environmental information and we do not know exactly when to apply position or force control in complicated environments. Unlike the HFPC method, the CFPC method can realize automatic smooth switching between position and force control according to the contact force feedback, and there is no overshoot of contact forces, as shown in Figure 14b. The results of position tracking are shown in Figure 14c, and the weights of position control and force control are shown in Figure 14d.

Figure 14. Force-tracking experiment on a complex surface. Force tracking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Finally, we conducted a grinding experiment that changes the position control to force control during the working process to further demonstrate the performance of the proposed CFPC in practical applications. The desired grinding force was set to 10 N on a Q235 plate with an initial roughness of 0.49 μm, and the motor speed was set to 3000 rpm. The grinding devices is shown in Figure 8c.

The grinding force obtained using CFPC approaches the desired force smoothly (Figure 15b), whereas the grinding force obtained using HFPC presents an overshoot (Figure 15a). As seen from Figure 15d, the control mode switches gradually. The results of the position tracking are shown in Figure 15c, and the grinding results are shown in Figure 16. The roughness values of the ground plates are 0.059 μm for HFPC and 0.031 μm for the proposed CFPC. The roughness is measured in the time interval [0, 8] s. The grinding experiment further demonstrates that the proposed CFPC can realize smooth switching between position and force control, which leads to better grinding performances.

Figure 15. Grinding experiment on the plane. Force tacking using (a) the HFPC and (b) proposed CFPC methods. (c) Position tracking. (d) Weighted parameter $s_{wi}$.

Figure 16. (a) Grinding experiment results and (b) measured roughness from the HFPC (left photograph) and proposed CFPC (right photograph) methods.

4. Conclusions

To realize smooth switching between a free space and a contact manipulating space, a CFPC method was proposed by combining the proposed adaptive impedance control method with the modified HFPC method. This control method can provide an automatic smooth transition between position and force control according to the contact force feedback, thereby solving the oscillation problem of traditional HFPC. Moreover, the proposed CFPC method has good force-tracking performance in complicated environments. The stability of the force controller has also been analyzed. The feasibility of the proposed CFPC method was evaluated through constant and variable force-tracking simulations, as well as physical experiments on sloped, curved, and complicated surfaces. A grinding experiment was conducted to compare the performances of the HFPC and the proposed CFPC methods, which showed that the proposed CFPC can provide better roughness compared to the HFPC method. The simulations and experiments demonstrated that the proposed CFPC method could realize automatic smooth switching between free space and interaction space without system oscillation and contact force overshoot.

Our future work will focus on further improving the response ability of the system.