1 Introduction

Industrial serial manipulators controlled in position mode play an important role in the manufacturing field. A major challenge for position-controlled manipulators is the unexpected interaction with the environment, which may damage the manipulator, environment, or operator. Furthermore, tasks such as multi-robot cooperation [1, 2], exoskeleton robots [3, 4], physical Human–Robot Interaction [5, 6], polishing [7,8,9], assembly [10], and collision detection [11, 12] require monitoring of the contact force. The typical method to monitor the contact force information is to employ highly sensitive force/torque sensors on the end effector or joint torque sensors. Depending on the location of these sensors, hybrid position/force control [13] and impedance control [14] are proposed and well developed in the past four decades.

However, the high price of these sensors limits their popularity in industry. In addition, the performance of the sensors can be affected by environmental factors such as temperature and humidity. In order to achieve force contact estimation without such sensors, various sensorless contact force estimation methods have been proposed. In [15,16,17,18,19], contact force estimation methods that utilize a disturbance observer are suggested. When the manipulator interacts with the environment, the exerted force is regarded as the disturbance and estimated by comparing the actual output to the output based on a nominal model. For nonlinear systems, the nonlinear disturbance observer provided by Chen [20] is widely used [21,22,23]. With these model-based approaches, the disturbance estimation accuracy is theoretically guaranteed. But in reality, the approaches are limited by the precision of the modelled mechanics. The drawback of model estimation is that the inversion of the manipulator inertia matrix must be calculated online, which will lead to extra computational burden. Furthermore, a constant disturbance has to be assumed, and that restricts its extensive use beyond research.

Recently, flexible manipulators such as rehabilitation robots are widely used and compliance elements are well developed since they improve backdrivability and safety during physical Human–Robot Interaction. Incorporating compliance elements, sensorless force estimation can be conducted by position measurement. For example, stiff and sensitive joint torque estimation is realized due to the structural elasticity of the robotic joints with harmonic drive transmission in [24,25,26], where only motor-side and link-side position measurements are required. However, this approach requires a harmonic drive compliance model before it is performed, which is very complicated to implement. Furthermore, compliant elements ultimately leading to high costs imply that direct force measurement is preferable in many cases. Other kinds of sensorless contact force estimation approaches, such as time delay estimation and virtual springs, can be found in [10] and [27], respectively.

Another method relying on expressing manipulator dynamics in the format of the generalized momentum is introduced by De Luca et al. [28,29,30]. This method was originally designed for force/position control and collision detection, but more recently, researchers have exploited its further application to lead-through programming [31]. Using motor current and Moore–Penrose generalized inverse matrix, Yuan et al. [32] estimate external force/torque on the end effector and apply it to a combined control mode. Similarly, motor current is monitored in [33] and transformed to joint torques, which is then used to estimate the external force. In [34], a novel collision detection method based on virtual powers indexes is studied. Essentially speaking, [32,33,34,35,36,37] all employ De Luca’s dynamic model in the generalized momentum format since it works independently of joint acceleration measurements and inversion of the manipulator inertia matrix. However, the research based on that model is limited to collision detection or low-precision force control because the noise in the measured current can lead to false-positive collision recognition or insufficient force estimation precision. In order to solve that problem, the Kalman filter is investigated and applied to contact force sensing with the generalized momentum model. In [38], an estimator, which utilizes the extended Kalman filter and a Lyapunov-based adaption law, is proposed to estimate the contact force. The proposed estimator is robust to model errors and sensor noise. In [39], the estimator is modified and assumptions like underlying patterns govern subject behavior are not required anymore. In [40], the authors propose a semiparametric dynamics model and train the nonparametric part with multilayer perception network. With the composite model, the contact force is regarded as a disturbance variable observed by the Kalman filter. The uncertainty in the dynamics is neglected since the unmodelled effects have been compensated by the nonparametric model. Consequently, empirical parameters are utilized when tuning the Kalman filter. With an automatic covariance matrix calibration scheme presented in [41], the problem of the neglected dynamic uncertainty in [40] is fixed. The scheme abstracts uncertainty in the system dynamics to improve the estimation quality. In order to apply the standard Kalman filter, a covariance matrix of the predicted state noise has to be calibrated. It is found that the smaller the diagonal elements of the covariance matrix are, the faster the Kalman filter responds to the external wrench. Simply, reducing the magnitude of the diagonal elements of the covariance matrix will be the key to reducing the response time. However, the covariance matrix in [41] is limited by the errors of the friction model, which are generally assumed constant. Furthermore, the scheme still depends on joint torque sensors when modelling the friction and the noise in the measured current is ignored.

In this paper, a contact force estimation method for a serial manipulator with no embedded force/torque sensors is proposed. The method extends existing research in terms of both dynamic modelling, uncertainty identification, and contact force estimation. The main contributions of this paper are the nature of the current sensing filter and validation of its capacity in contact force estimation with comprehensively exploited dynamic information.

The proposed approach uses a real-time Weighted Moving Average (WMA) with variable span to reduce the apparent noise in the measured motor current and the magnitude of the diagonal elements of the covariance matrix. Due to the automatically varying span, the trends in the smoothed motor current signal are preserved when the motor output changes. The proposed filter is then combined with the automatic tuning method of the standard Kalman filter (SKF) and applied to online force estimation. The estimation accuracy is improved and the response time is decreased in spite of the time lag of the WMA, which is typical of all non-predictive moving averages.

In this paper, Sect. 2 gives the generalized momentum–based model and its discretization methods. The implementation of the SKF is also described in this section. Sect. 3 presents the sensorless identification method of the dynamic model, including the motor torque constant and the Stribeck friction model. Calibration of the SKF and the variable span of the WMA is undertaken. In Sect. 4, experiments on the Universal Robot 5 (UR5) manipulator with the proposed contact force estimation method and the experimental results are shown. The experimental results and the effectiveness of the proposed approach are discussed in Sect. 5. Sect. 6 gives conclusion.

2 Dynamic modelling of the manipulator and application of the standard Kalman filter

In this research, the UR5 (as shown in Fig. 1) is employed to perform contact force estimation.

Fig. 1
figure 1

Universal Robot 5 (UR5). (left) 3-D model. (right) Corresponding kinematic model

Full size image

2.1 Modelling of UR5 based on the generalized momentum

The rigid body dynamics of UR5 can be described as.

$${\varvec{M}}\left( \varvec{{q}} \right){\varvec{\ddot{q}}} + {\varvec{CC}}\left( {{\varvec{q}},\varvec{\dot{{q}}}} \right)\varvec{\dot{{q}}} + \varvec{{g}}\left( \varvec{{q}} \right) + \varvec{{\tau }}_{{{\rm{fri}}}} + \varvec{{\tau }}_{{{\rm{ext}}}} + {\varvec{\tau} }_{{{\rm{dri}}}} = 0$$
(1)

For a serial manipulator with \({{n}}\) degrees of freedom, \({\varvec{M}}\left( {\varvec{q}} \right) \in \Re^{n \times n}\) represents the inertia matrix. \(\varvec{CC}\left( {{\varvec{q}},\varvec{\dot{{q}}}} \right) \in \Re^{n \times n}\) denotes the Coriolis and centrifugal matrix. \(\varvec{{g}}\left( {\varvec{q}} \right) \in \Re^{n \times n}\) is the gravity effect of the manipulator exerted in the joints. Friction torques in the joints are captured by \({\varvec{\tau }}_{{{\rm{fri}}}} \in \Re^{n}\). \({\varvec{\tau} }_{{{\rm{dri}}}} \in \Re^{n}\) stands for the joint torques that drive the manipulator. \({\varvec{q}} \in \Re^{n}\), \(\varvec{\dot{{q}}} \in \Re^{n}\), and \(\varvec{{\ddot{q}}} \in \Re^{n}\) are joint position, velocity, and acceleration, respectively. The joint torques, which result from external contact forces, are expressed as \(\varvec{{\tau }}_{{{\rm{ext}}}} \in \Re^{n}\). The external wrench \(\varvec{{f}} = \left[ {f_{x} \, f_{y} \, f_{z} \, \tau_{x} \, \tau_{y} \, \tau_{z} } \right]^{T} \in \Re^{m}\) is related to \(\varvec{{\tau }}_{{{\rm{ext}}}}\) by the manipulator Jacobian \(\varvec{{J}}(\varvec{{q}}) \in \Re^{n \times m}\) (where \({{m}}\) is the dimension of the external force). The related equation is expressed as \(\varvec{{\tau }}_{{{\rm{ext}}}} = \varvec{{J}}^{T} \left( \varvec{{q}} \right)\varvec{{f}}\), where \(f_{x}\), \(f_{y}\), and \(f_{z}\) are the contact forces that occur at the tool center point (TCP) in \(x\), \(y\), and \(z\) directions of the base frame and \(\tau_{x}\), \(\tau_{y}\), and \(\tau_{z}\) are the corresponding interaction torques. It is notable that \(m \le 6\) can happen when external forces or torques in certain directions are null vectors. Only external forces exerted at the TCP are considered in this research. The (kinematic) Denavit–Hartenberg and dynamic parameters of the UR5 are summarized in Tables 1 and 2, respectively.

Table 1 Kinematic parameters of UR5
Full size table
Table 2 Dynamic parameters of the UR5
Full size table

In the general case, calculation of \(\varvec{f}\) exerted on the manipulator relies on the Euler–Lagrange formulation of the manipulator dynamics described in (1). However, this method fails when real-time estimation of the external wrench is conducted, since joint acceleration \(\varvec{\ddot{q}}\) is obtained online by numerical differentiation of the measurable joint velocity \(\varvec{\dot{{q}}}\), which will lead to amplification of the measurement noise. The dynamic model expressed in the format of the generalized momentum in [11] avoids the difficulty and the generalized momentum \({\varvec{p}}\) of the manipulator is defined as \({\varvec{p}} = {\varvec{M}}\left( {\varvec{q}} \right)\varvec{\dot{{q}}}\).

The derivate of the generalized momentum with respect to time is obtained as.

$$\varvec{\dot{{p}}} = \varvec{\dot{{M}}}\left( {\varvec{q}} \right)\varvec{\dot{{q}}} + {\varvec{M}}\left( {\varvec{q}} \right)\varvec{{\ddot{q}}}$$
(2)

Following the fact in [42] \(\varvec{\dot{{M}}}\left( {\varvec{q}} \right) - 2{\varvec{CC}}\left( {{\varvec{q}},\varvec{\dot{{q}}}} \right) = - \left( {\varvec{\dot{{M}}}\left( {\varvec{q}} \right) - 2{\varvec{CC}}\left( {\varvec{q},\varvec{\dot{q}}} \right)} \right)^{T}\), Eq. (1) can be converted into.

$$\varvec{\dot{{p}}} = \varvec{{CC}}\left( \varvec{{q},\varvec{\dot{{q}}}} \right)^{T} \varvec{\dot{{q}}} - \varvec{{g}}\left( \varvec{{q}} \right) - \varvec{{\tau }}_{{{\rm{fri}}}} - \varvec{{\tau }}_{{{\rm{dri}}}} - \varvec{{\tau }}_{{{\rm{ext}}}}$$
(3)

Defining the term \(\varvec{{\tau }}_{{{\rm{inp}}}} = \varvec{{CC}}\left( {\varvec{q},\varvec{\dot{{q}}}} \right)^{T} \varvec{\dot{{q}}} - \varvec{g}\left( \varvec{q} \right) - \varvec{\tau }_{{{\rm{fri}}}} - \varvec{\tau }_{{{\rm{dri}}}}\) and substituting it into (1), the dynamics of the manipulator can be written as

$$\varvec{\dot{{p}}} = {\varvec{\tau}}_{{{\text{inp}}}} - {\varvec{J}}\left( {\varvec{q}} \right)^{T} {\varvec{f}} + {\varvec{e}}_{p}$$
(4)

where \(\varvec{e}_{{{p}}} \sim N\left( {0,\varvec{\sigma }_{{\rm {Dyn}}}^{2} } \right)\) denotes the noise resulting from \(\varvec{\tau }_{{{\rm{inp}}}}\). \(\varvec{\sigma }_{{\rm {Dyn}}}^{2}\) is assumed as a diagonal matrix reflecting that errors in the individual joints are independent and \(\varvec{\sigma }_{{\rm {Dyn}}}^{2} \in \Re^{n \times n}\). In [41], the uncertainty of the friction model is regarded as the dominant part in the manipulator dynamic model. However, the term \({\varvec{\tau}}_{{\rm{dri}}}\) included in (4) is obtained based on the noisy current. It is reasonable to consider the uncertainty in the measured current as the dominant noise in (4) and the details can be found in Sect. 3.

2.2 Discretization of the UR5 dynamics

The dynamics of the external wrench is defined as.

$$\varvec{\dot{{f}}} = {\varvec{{S}}\varvec{f}} + \varvec{e}_{{{f}}}$$
(5)

where \({\varvec{S}} \in \Re^{m \times m}\) is the dynamic matrix of the external wrench. The uncertainty of the external wrench is assumed to be normally distributed as \(\varvec{e}_{{{f}}} \sim N\left( {0,\varvec{\sigma }_{{{f}}}^{2} } \right)\), \(\varvec{\sigma }_{{{f}}}^{2}\) is assumed as a diagonal matrix reflecting that uncertainty of the external wrench in each direction is independent and \(\varvec{\sigma }_{{{f}}}^{2} \in \Re^{n \times n}\). Intuitively, \(\varvec{S}\) is determined according to the external wrench. However, the prior information of the external forces/torques is not available when the manipulator interacts with the unexpected environment. Hence, \({\varvec{S}}\) is defaulted as \(0\) in this approach and \(\varvec{\dot{{f}}} = \varvec{e}_{{{f}}}\). The default is appropriate when constant contact estimation is conducted and it also works well with varying external wrench since large diagonal elements of \(\varvec{\sigma }_{{{f}}}^{2}\) are employed in our research in order to avoid the assumption that the contact force is constant. However, the diagonal elements must be limited to a certain magnitude as larger diagonal elements will result in increased noise amplification. The situation is improved in our research due to the introduction of the WMA. In other words, during the same response period (with the same \(\varvec{\sigma }_{{{f}}}^{2}\)), the noise is less than that resulting from the SKF.

With (4) and (5), the state vector can be defined as \({\varvec{x}} = \left[ {{\varvec{p}},{\varvec{f}}} \right]^{T} \in \Re^{n + m}\). The generalized momentum \({\varvec{p}}\) is available since both \(\varvec{q}\) and \(\varvec{\dot{{q}}}\) can be measured and thus the output \(\varvec{y}\) of the system is obtained. The state model of the manipulator is expressed as

$$\begin{gathered} \varvec{\dot{{x}}} = \varvec{Ax} + \varvec{Bu} + \varvec{w} \hfill \\ \varvec{y} = \varvec{Cx} + \varvec{Z}{.} \hfill \\ \end{gathered}$$
(6)

where \({\varvec{A}} = \left[ {\begin{array}{*{20}c} {0_{n \times n} } & { - {\varvec{J}}({\varvec{q}})^{T} } \\ {0_{n \times n} } & {\varvec{S}} \\ \end{array} } \right]\), \({\varvec{B}} = \left[ {\begin{array}{*{20}c} {{\varvec{I}}_{n} } \\ {0_{m \times n} } \\ \end{array} } \right]\), and \({\varvec{C}} = \left[ {\begin{array}{*{20}c} {{\varvec{I}}_{n} } & {0_{n \times m} } \\ \end{array} } \right]\) are the state matrices. \({\varvec{w}} = \left[ {\begin{array}{*{20}c} {{\varvec{e}}_{{{p}}}^{T} } & {{\varvec{e}}_{{{f}}}^{T} } \\ \end{array} } \right]^{T} \in \Re^{n + m}\) denotes the covariance matrix of the predicted state noise. \({\varvec{u}} = {\varvec{\tau}}_{{\rm {inp}}}\) is the input of the state model and the measurement noise is \({\varvec{Z}}\sim N\left( {0,{\varvec{\sigma}}_{{\rm {mea}}}^{2} } \right)\) with \({\varvec{\sigma}}_{{{\rm{mea}}}}^{2} \in \Re^{n \times n}\). The upper part of Eq. (6) is intended to allow the approach to manage uncertain loads.

The SKF is applied to the discretized system and thus, the state space dynamics (6) are discretized as

$$\begin{gathered} {\varvec{x}}_{k + 1} = {\varvec{A}}_{k + 1} {\varvec{x}}_{k} + {\varvec{B}}_{k + 1} {\varvec{u}}_{k + 1} + {\varvec{w}}_{k + 1} \hfill \\ {\varvec{y}}_{k + 1} = {\varvec{C}}_{k + 1} {\varvec{x}}_{k + 1} + {\varvec{z}}_{k + 1} \hfill \\ \end{gathered}$$
(7)

where \(k\) and \(k + 1\) are the time steps of the discrete system. With the method proposed in [43], the discretized matrices \({\varvec{A}}_{k + 1}\), \({\varvec{B}}_{k + 1}\), and \({\varvec{C}}_{k + 1}\) are obtained by

$$\begin{gathered} \left[ {\begin{array}{*{20}c} {{\varvec{A}}_{k + 1} } & {{\varvec{B}}_{k + 1} } \\ {0_{n \times (n + m)} } & {{\varvec{I}}_{n} } \\ \end{array} } \right] = \exp \left( {\left[ {\begin{array}{*{20}c} {\varvec{A}} & {\varvec{B}} \\ {0_{n \times (n + m)} } & {0_{n \times n} } \\ \end{array} } \right]t_{{s}} } \right) \hfill \\ {\varvec{C}} = {\varvec{C}}_{k + 1} \hfill \\ \end{gathered}$$
(8)

The measurement noise covariance matrix is calculated by

$${\varvec{\sigma}}_{k + 1}^{2} = {\varvec{\sigma}}_{{\rm{mea}}}^{2} /t_{s}$$
(9)

where \(t_{s}\) is the sampling time. Following [44], the discretized predicted state noise matrix \({\varvec{Q}}_{k + 1}\) is given by

$$\begin{gathered} \left[ {\begin{array}{*{20}c} {{\varvec{M}}_{k + 1}^{11} } & {{\varvec{M}}_{k + 1}^{12} } \\ {0_{(n + m) \times (n + m)} } & {{\varvec{M}}_{k + 1}^{22} } \\ \end{array} } \right] = \exp \left( {\left[ {\begin{array}{*{20}c} {\varvec{A}} & {{\varvec{\sigma}}_{{\rm {pro}}}^{2} } \\ {0_{(n + m) \times (n + m)} } & { - {\varvec{A}}} \\ \end{array} } \right]t_{s} } \right) \hfill \\ {\varvec{Q}}_{k + 1} = ({\varvec{M}}_{k + 1}^{22} )^{T} {\varvec{M}}_{k + 1}^{12} \hfill \\ \end{gathered}$$
(10)

where the covariance matrix of the predicted state noise is expressed as \({\varvec{\sigma}}_{{\rm {pro}}}^{2} = \left[ {\begin{array}{*{20}c} {{{\varvec{\sigma}}}_{{\rm {Dyn}}}^{2} } & {0_{m \times n} } \\ {0_{n \times m} } & {{\varvec{\sigma}}_{f}^{2} } \\ \end{array} } \right]\).

2.3 Implementation of the standard Kalman filter

With (7)–(10), the SKF is implemented as follows:

Step 1 Initialize the state vector \({\varvec{x}}_{0}\) and covariance matrix \({\varvec{P}}_{0}\).

In the general case, the initial state vector and the initial covariance matrix are defaulted as \({\varvec{x}}_{0} = 0_{{\left( {m + n} \right) \times 1}}\) and \({\varvec{P}}_{0} = {\varvec{I}}_{m + n}\), respectively. However, a rough estimation can be considered the initial value if it is known a priori.

Step 2 Discretize the system matrices.

\({\varvec{A}}\) is a time-varying matrix since it contains the joint position–based manipulator Jacobian \({\varvec{J}}\left( {\varvec{q}} \right)\). As a result, the system matrices have to be discretized in each circulatory computation following (8)–(10).

Step 3. Predict the state vector and the covariance matrix.

$$\begin{gathered} \overline{{{\varvec{x}}_{k + 1} }} = {\varvec{A}}_{k + 1} {\varvec{x}}_{k} + {\varvec{B}}_{k + 1} {\varvec{u}}_{k + 1} \hfill \\ \overline{{{\varvec{P}}_{k + 1} }} = {\varvec{A}}_{k + 1} {\varvec{P}}_{k} {\varvec{A}}_{k + 1}^{T} + {\varvec{Q}}^{k + 1} \hfill \\ \end{gathered}$$
(11)

Step 4 Update the Kalman gain.

$${\varvec{K}}_{k + 1} = \overline{{{\varvec{P}}_{k + 1} }} {\varvec{C}}_{k + 1}^{T} /\left( {{\varvec{C}}_{k + 1} \overline{{{\varvec{P}}_{k + 1} }} {\varvec{C}}_{k + 1}^{T} + {\varvec{\sigma}}_{k + 1}^{2} } \right)$$
(12)

Step 5 Update the state vector and covariance matrix with the measurement.

$$\begin{gathered} {\varvec{x}}_{k + 1} = \overline{{{\varvec{x}}_{k + 1} }} + {\varvec{K}}_{k + 1} \left( {{\varvec{y}}_{k + 1} - {\varvec{C}}_{k + 1} \overline{{{\varvec{x}}_{k + 1} }} } \right) \hfill \\ {\varvec{P}}_{k + 1} = \left( {{\varvec{I}} - {\varvec{K}}_{k + 1} {\varvec{C}}_{k + 1} } \right)\overline{{{\varvec{P}}_{k + 1} }} \hfill \\ \end{gathered}$$
(13)

Step 6 Compute the external wrench and then repeat Step 2.

$${\varvec{f}}_{k + 1} = \left[ {\begin{array}{*{20}c} {0_{m + n} } & {{\varvec{I}}_{m} } \\ \end{array} } \right]{\varvec{x}}_{k + 1}$$
(14)

3 Identification of the dynamic parameter and tuning of the standard Kalman filter

In order to implement the SKF, the dynamic model of the manipulator must be defined and identification of the unknown dynamic parameters must be performed. Furthermore, the predicted state noise matrix \({\varvec{\sigma}}_{{\rm {Dyn}}}^{2}\), \({\varvec{\sigma}}_{f}^{2}\), and the measurement covariance matrix \({\varvec{\sigma}}_{{\rm {mea}}}^{2}\) must be calibrated. In [31] and [40], the motor constant and gearbox ratio are given and therefore the dynamic model of \({{\varvec{\tau}}}_{{\rm {dri}}}\) is obtained. Furthermore, in [6] and [40], joint torque sensors are used to calibrate the friction model. However, joint torque sensors are not used in this research and an alternative is proposed. For simplification, the elbow joint of the UR5 is conducted to display how the identification approach works and the sketch of the UR5 is shown in Fig. 2.

Fig. 2
figure 2

Sketch of the UR5

Full size image

3.1 Identification of the motor torque and friction

It is to be noticed that, in this brief, only the motor current \({\varvec{i}}\), joint position \(\varvec{q}\), and joint velocity \(\varvec{\dot{{q}}}\) are measurable. The joint driving torque is calculated by

$$\widehat{{{\tau}}_{\rm {elbow}}}={{i}}_{\rm {elbow}}{k}_{\rm {elbow}}{r}_{\rm {elbow}}$$
(15)

where \(\widehat{{{\tau}}_{\rm {elbow}}}\), \({i}_{{{\rm {elbow}}}}\), \(k_{{{\rm {elbow}}}}\), and \(r_{{{\rm {elbow}}}}\) denote the driving torque, motor current, motor torque constant, and gearbox ratio of the elbow joint, respectively. A composite term is subsequently defined as

$${c}_{{{\rm {elbow}}}} = {k}_{{{\rm {elbow}}}} {r}_{{{\rm {elbow}}}}$$
(16)

The term \({c}_{{{\rm {elbow}}}}\) of the elbow joint is determined experimentally by moving the elbow joint at a constant speed clockwise and counterclockwise while the other joints stay stationary (as shown in Fig. 3).

Fig. 3
figure 3

Identification of the motor torque constant and friction

Full size image

When the elbow joint rotates clockwise and counterclockwise without external wrench, the manipulator dynamics described in terms of current can be expressed as

$$\begin{gathered} {\varvec{i}}_{{{\rm{gra}}}}^{{}} + {\varvec{i}}_{{{\rm{fri}}}}^{{}} + {\varvec{i}}_{{{\rm{dri}}}}^{ + } = 0 \hfill \\ {\varvec{i}}_{{{\rm{gra}}}}^{{}} - {\varvec{i}}_{{{\rm{fri}}}}^{{}} + {\varvec{i}}_{{{\rm{dri}}}}^{ - } = 0 \hfill \\ \end{gathered}$$
(17)

where \({\varvec{i}}_{{{\rm{gra}}}}^{{}}\) is the current related to the elbow joint torque resulting from the gravity of Link 3, Link 4, Link 5, Link 6, Wrist 1, Wrist 2, and Wrist 3. \({\varvec{i}}_{{{\rm{fri}}}}^{{}}\) denotes the current corresponding to the elbow joint friction. \({\varvec{i}}_{{{\rm{dri}}}}^{ + }\) and \({\varvec{i}}_{{{\rm{dri}}}}^{ - }\) indicate the motor currents with the elbow joint moving clockwise and counterclockwise, respectively. With (17), \({\varvec{i}}_{{{\rm{gra}}}}^{{}}\) is calculated

$${\varvec{i}}_{{{\rm{gra}}}}^{{}} = \left( { - {\varvec{i}}_{{{\rm{dri}}}}^{ + } - {\varvec{i}}_{{{\rm{dri}}}}^{ - } } \right)/2.$$
(18)

An accurate dynamic model is assumed and thus the term \({c}_{{{\rm {elbow}}}}\) can be obtained by

$${c}_{{{\rm {elbow}}}} = {g}_{{{\rm{elbow}}}} /{i}_{{{\rm{gra}}}}^{{}}$$
(19)

where \({g}_{{{\rm{elbow}}}}\) is the elbow joint torques resulting from gravity in (1).

Generally, the friction increases discontinuously as the relative velocity of the contact surfaces increases. Therefore, the Stribeck model is utilized to express the nonlinearity of the friction. Following [45], the Stribeck model can be described as

$${\varvec{f}}(\varvec{\dot{{q}}}) = {\varvec{f}}_{{c}} + \left( {{\varvec{f}}_{{s}} - {\varvec{f}}_{{c}} } \right)\exp \left( { - \left| {\varvec{\dot{{q}}}/\varvec{\dot{{q}}}_{{s}} } \right|{\varvec{\delta}}_{{s}} } \right) + {\varvec{f}}_{{\dot{{q}}}} \varvec{\dot{{q}}}$$
(20)

where \({\varvec{f}}_{{c}}\) stands for the Column friction, \({\varvec{f}}_{{s}}\) is the static friction, \(\varvec{\dot{{q}}}_{{s}}\) represents the Stribeck velocity, \({\varvec{\delta}}_{{s}}\) denotes an empirical parameter, and \({\varvec{f}}_{{\dot{{q}}}}\) is the viscous friction.

With (17) and (19), the friction of the elbow joint moving at different speed is obtained.

$${\varvec{f}}(\varvec{\dot{{q}}}) = \left( {{\varvec{i}}_{{{\rm{dri}}}}^{ - } - {\varvec{i}}_{{{\rm{dri}}}}^{ + } } \right){\varvec{c}}_{{{\rm{elbow}}}} /2$$
(21)

Consequently, the unknown parameters in (20) are identified.

3.2 The Weighted Moving Average with variable span and covariance matrix of predicted state noise

In order to implement the SKF, the predicted state noise \({\varvec{w}}\) and the measurement noise \(\varvec{Z}\) must be calibrated. Note that the term \({\varvec{\tau}}_{{{\rm{inp}}}}\) contains the noise caused by the measurement of \(\varvec{{q}}\),\(\dot{\varvec{q}}\), and \(\varvec{i}\). In contrast with [41] where the noise resulting from \({\varvec{q}}\) and \(\varvec{i}\) is ignored, all the noise is regarded as a combination and analyzed in this research. Experiments without external wrench are performed. Since the joint acceleration is unmeasurable, the individual joints move at constant speed in the experiments and the ideal joint output torques are calculated by

$$\overline{{{\varvec{\tau}}_{{{\rm{dri}}}} }} = - {\varvec{CC}}\left( {{\varvec{q}},\varvec{\dot{{q}}}} \right)\varvec{\dot{{q}}} - {\varvec{g}}\left( {\varvec{q}} \right) - {\varvec{\tau}}_{{{\rm{fri}}}} .$$
(22)

The UR5 manipulator consists of DC motors. The mean of the motor current can be assumed as the valid value. Based on the smoothed currents, the measured joint output torques are calculated in the same way as that in (15). In order to smooth the currents, a Weighted Moving Average with variable span is proposed (as shown in Fig. 4).

Fig. 4
figure 4

The weighted moving average based on variable span

Full size image

where \({i}_{d}\) is the measured current at step time \(d\). \(r\) and \(d\) denote the step times. \(t\) is the number of the samples which are averaged in each step and the detail of how \(t\) is determined is described in the following part. \({i}_{d}^{opt}\) stands for the filtered current at step time \(d\). \({\delta}\) is an experimentally determined parameter which is used to decide whether the span varies or not. \(W\left( {d,t} \right)\) is the Weighted Moving Average and is calculated by

$$W\left( {d,t} \right) = {{2\left[ {i_{d} t + i_{d - 1} \left( {t - 1} \right) + ... + i_{d - t + 1} } \right]} \mathord{\left/ {\vphantom {{2\left[ {i_{d} t + i_{d - 1} \left( {t - 1} \right) + ... + i_{d - t + 1} } \right]} {\left[ {t\left( {t + 1} \right)} \right]}}} \right. \kern-\nulldelimiterspace} {\left[ {t\left( {t + 1} \right)} \right]}}.$$
(23)

In this research, We assume that the time gap between any two detected changes of the current is larger than \(t_{s} \cdot t\).

Implementation of the WMA with variable span is as follows:

Step 1 At time step \(d\)(\(1 \le d < t\)), \(d\) samples (\({i}_{d}\) included) before \({i}_{d}\) are averaged to get the optimal option \({i}_{d}^{opt} = W\left( {d,d} \right)\) (as shown in Fig. 5).

Fig. 5
figure 5

d samples (id included) before id are in the dotted-line rectangle

Full size image

Step 2 At time step \(d\)(\(t \le d\)), \(t\) samples (\({{i}}_{d}\) included) before \({i}_{d}\) are averaged to get the optimal option \({i}_{d}^{opt} = W\left( {d,t} \right)\). At the same time, \({{i}}_{d + 1}\) is checked to make sure whether external force occurs (as shown in Fig. 6). If external force is detected, goes to Step 3; otherwise, \({i}_{d + 1}^{opt}\) is calculated in a similar manner to Step 2.

Fig. 6
figure 6

t samples (id included) before id are in the dotted-line rectangle. id+1 in the dashed-line rectangle is checked

Full size image

Step 3 In case of dramatic change in the external wrench or joint acceleration at time step \(d + 1\), a sudden rise or drop will occur in the current. If the change between the present current \({i}_{d + 1}\) and the previous currents \({i}_{d}^{opt}\) exceeds \(\delta\), an external wrench is assumed. Hence, from time step \(d + 1\) to time step \(d + t\), \(b\) samples (\({i}_{d + b}\) included, \(1 \le b < t\)) before \({i}_{d + b}\) are averaged to get the optimal option \({i}_{d + b}^{opt} = W\left( {d + b,b} \right)\) (as shown in Fig. 7).

Fig. 7
figure 7

b samples (id+b included) before id+b are in the dotted-line rectangle

Full size image

Step 4 At time step \(d + t\), the filter goes back to Step 2. \({i}_{d + t}^{opt} = W\left( {d + t,t} \right)\) is calculated and verification of \({i}_{d + t + 1}\) is performed (as shown in Fig. 8).

Fig. 8
figure 8

t samples (id+t included) before id+t are in the dotted-line rectangle. id+t+1 in the dashed-line rectangle is checked

Full size image

The errors between the target joint output torques \(\overline{{{\varvec{\tau}}_{{\rm{dri}}} }}\) and the optimized torques \(\widehat{{{\varvec{\tau}}}_{\rm {dri}}}\) calculated from the filtered current are analyzed with the Anderson–Darling test. In particular, the base joint of the UR5 is commanded to move without external forces/torques while the current is recorded and processed by the WMA with span \(t\). The errors are shown in terms of relative frequency in Fig. 9. The Anderson–Darling test verifies the appearance of a Gaussian distribution in Fig. 9. The variance of the errors is then obtained from the empirical standard deviation (SD).

Fig. 9
figure 9

Identification of the predicted state noise \({\varvec{e}}_{p}\). The base joint is employed as an example and moves without payload. The relative frequency of the errors is described as a histogram. The errors are computed with \(\overline{{{\varvec{\tau}}_{{\mathrm{dri}}} }}\) obtained from (22) and \(\widehat{{{\varvec{\tau}}}_{\rm {dri}}}\) obtained from (15). The red line is an added fitted line for the normal distribution, the variance of which is computed from the empirical standard deviation of the errors

Full size image

The covariance matrix \({\varvec{\sigma}}_{{{\rm{Dyn}}}}^{2}\) of the predicted state noise \({\varvec{e}}_{{p}}\) is further researched. For the individual joints, the variance varies as the span \(t\) changes. Therefore, identification experiments for each joint with different \(t\) have to be performed before conducting the force estimation. For example, the elbow joint is commanded to move at constant speed and the current was recorded. Figure 10 shows the dependence of the variance in the joint output torque errors on different spans. The variance drops with increasing \(t\) up to approximately \(t = 25\), after which there are negligible changes.

Fig. 10
figure 10

The variance of the predicted state noise \({\varvec{e}}_{p}\) with different spans. The green circles stand for the noise which is normally distributed. The red crosses denote the noise that does not follow normal distribution

Full size image

It is assumed that a greater number of samples in each averaging step will lead to a lower noise contribution in the current signal. However, excessive samples in each step will weaken the trends in the smoothed motor current signal because the actual current is changing all the time. Hence, the variance initially decreases as \(t\) increases. After that, the variance in the joint output torque errors plateaus as the smoothing benefits are cancelled by the latency of the system. Worse yet, excessive examples in each averaging step can lead to new errors that are not Gaussian. In that case, the biggest span of \(t = 19\), with which the noise is still normally distributed for the elbow joints, is determined and the related variance is obtained.

3.3 Measurement noise matrix

In order to extract the measurement noise, the elbow joint of the UR5 manipulator is moved at constant velocity as an example, the empirical distribution of the errors between the target speed \(\overline{\varvec{\dot{{q}}}}\) and the measured mean speed \(\widehat{{\varvec{\dot{{q}}}}}\) is shown in Fig. 11. With the Anderson–Darling test, it can be reasonably concluded that the actual speed is normally distributed with\(\dot{{\varvec{q}}}\sim N\left(\overline{\dot{{\varvec{q}}} },{{\varvec{\sigma}}}_{\dot{{{q}}}}^{2}\right)\). The standard deviation of the elbow joint \({\sigma}_{{\dot{q}}}^{{\rm {elbow}}}\) is observed to vary as the speed changes and the trend is depicted in Fig. 12. As shown in Fig. 12, \({\sigma}_{{\dot{q}}}^{{\rm {elbow}}}\) increases approximately linearly with the velocity from speed 0 to 0.13 rad/s and stays constant above that speed. Similarly, the variance corresponding to the individual joints velocity is obtained.

$$\left( {{\sigma}_{{\dot{{q}}}}^{{i}} } \right)^{2} = \left\{ {\begin{array}{*{20}c} {\left( {a\dot{{q}}^{i} + b} \right)^{{2}} ,\left| {\dot{{q}}^{i} } \right| \le \dot{{q}}^{i}_{{\rm {thr}}} } \\ {c^{{2}} ,\left| {\dot{{q}}^{i} } \right| > \dot{{q}}_{{\rm {thr}}}^{i} } \\ \end{array} } \right.$$
(24)
Fig. 11
figure 11

Identification of the measurement noise matrix. The elbow joint is employed as an example and moves at constant speed. The relative frequency of the errors is described as a histogram. The errors are computed with the target speed \(\overline{{\varvec{\dot{{q}}}}}\) and the measured mean speed \(\widehat{{\varvec{\dot{{q}}}}}\). The red line is an added fitted line for the normal distribution, the variance of which is computed from the empirical standard deviation of the errors

Full size image
Fig. 12
figure 12

The standard deviation of the elbow joint speed noise at different speeds

Full size image

where \(\dot{{q}}_{{\rm {thr}}}^{i}\) is a threshold velocity for the \(i\) joint of the manipulator. \(a\), \(b\), and \(c\) are obtained with offline identification experiment.

The dynamic model of the manipulator is assumed to be accurate, and thus the measurement noise is caused by only the measured terms \(\varvec{q}\) and \(\varvec{\dot{{q}}}\). The noise in (6) can thus be expressed by

$${\varvec{Z}}\sim N\left( {0,{\varvec{\sigma}}_{{{\rm{mea}}}}^{2} } \right)$$
(25)

where \({\varvec{\sigma}}_{{{\text{mea}}}}^{2} = {\varvec{M}}\left( {\varvec{q}} \right){\varvec{\sigma}}_{{\dot{{q}}}}^{2} {\varvec{M}}\left( {\varvec{q}} \right)\).

4 Experiments and results

4.1 Experiments

The UR5 manipulator is used and moves to an initial position with a gripper attached as shown in Fig. 13a. After initialization, the gripper is commanded to move up at constant speed with no payload. After 33.6 mm of motion, the gripper engages an unexpected payload (a lead-filled cup) as shown in Fig. 13b, and begins to lift the payload (Fig. 13c). The experiment is repeated with payloads of 400 to 900 g with increments of 100 g and from 1000 to 2000 g with increments of 200 g. Furthermore, experiments with the TCP moving with the constant payload but at different speeds (0.04 m/s, 0.06 m/s, and 0.08 m/s, respectively) are conducted. Moreover, experiments with the UR5 moving in different orientation from that in Fig. 13 are performed. At last, experiments of estimating varying contact with the SKFW and the standard Kalman filter based on the Weighted Moving Average with no variable span (SKFNW) are performed.

Fig. 13
figure 13

a–c Demonstration scenario for the experiment. The UR5 manipulator is initialized as a. The TCP is commanded to move up vertically and the effect of the cup filled with lead can be regarded as constant external wrench exerted on the TCP

Full size image

During the movement, the joint kinematic information and motor current are recorded with a sampling frequency of 125 Hz. The effect of the gripper attached on the end of the manipulator and the payload is regarded as a constant force in the \(- Z{0}\) direction.

4.2 Results

Figure 14 shows the force estimation performance with methods of the SKF presented in [40] and the proposed estimation approach of the standard Kalman filter based on the Weighted Moving Average with variable span (SKFW). The torque estimation results are displayed in Fig. 15.

Fig. 14
figure 14

Estimation of contact force (with 1400 g lead inside the cup) in the –Z0 direction. The applied force (black curve) is compared to estimation with the SKF in [40] (green curve) and the SKFW (red curve). The dashed black line indicates the time when external force is exerted on the TCP. The dashed green and red ones show the response time of the two estimation methods

Full size image
Fig. 15
figure 15

Estimation of contact torque (with 1000 g lead inside the cup) in the X0 direction. The applied torque (black curve) is compared to estimation with the SKF in [40] (green curve) and the SKFW (red curve). The dashed black line indicates the time when external torque is exerted on the TCP. The dashed green and red ones show the response time of the two estimation methods

Full size image

As the payload varies, the root-mean-squared errors (RMSE) of the force estimation based on the SKF and the SKFW are displayed in Fig. 16.

Fig. 16
figure 16

The RMSE of the external force estimation based on the SKF and the SKFW with the lead changing from 400 to 1000 g with increments of 100 g and from 1000 to 2000 g with increments of 200 g. The green curve stands for the RMSE of the force estimation based on the SKF. The red curve denotes the RMSE of the force estimation based on the SKFW

Full size image

The response time of force sensing based on both methods varies with the payload changes as shown in Fig. 17.

Fig. 17
figure 17

Comparison of the response time with different payloads. The green curve and the red one stand for the force estimation based on SKF and SKFW, respectively

Full size image

The variance of the estimation errors of the approaches using the same payload and at different speeds are compared in Tables 3 and 4.

Table 3 Variance [N2] of the force estimation errors with different motions
Full size table
Table 4 Variance [(Nm)2] of the torque estimation errors with different motions
Full size table

Figure 18 shows the performance of the comparison experiments with the UR5 in the alternative orientation. Both methods are employed and their estimation results are displayed. The force estimation results based on the SKFNW and the SKFW are displayed in Fig. 19.

Fig. 18
figure 18

Force estimation results of the comparison experiments (in different orientation from that in Fig. 13) in the –Z0 direction (with 1000 g lead inside the cup). The applied force (black curve) is compared to estimation with the SKF in [40] (green curve) and the SKFW (red curve). The dashed black line indicates the time when external force is exerted on the TCP. The dashed green and red ones show the response time of the two estimation methods

Full size image
Fig. 19
figure 19

Estimation of contact force (with 1400 g lead inside the cup) in the –Z0 direction. The applied force (black curve) is compared to estimation with the SKFNW (blue curve) and the SKFW (red curve). The dashed black line indicates the time when external force is exerted on the TCP. The dashed blue and red ones show the response time of the two estimation methods

Full size image

5 Discussion

The proposed contact force estimation method based on the SKFW works effectively in the physical experiments and the response time is reduced by 55.2% on average compared to an established SKF approach [40] (Fig. 17). Furthermore, the root-mean-square error between the estimator force and the measured contact force is reduced by 20.8% (Fig. 16). As to the torque estimation, the estimation errors of the SKFW are smaller than those from the SKF (Table 4). However, the response time of the SKFW is similar to that of the SKF (Fig. 15).

5.1 Influence of the WMA and the variable span

The proposed SKFW method is validated with the TCP of the UR5 moving at different speeds (Tables 3 and 4) and in different orientations (Fig. 18). The proposed method ultimately produces force and torque estimation errors that are smaller than the established method.

As displayed in Fig. 19, the SKFW works faster than the SKFNW when dealing with the varying contact, which validates the function of the variable span. When external force is exerted on the manipulator, the previous current samples are omitted in each averaging step because of the variable span. The benefit is twofold: Firstly, the fixed span approach has a longer response time. Secondly, estimation errors are reduced significantly by the proposed approach since there are apparent gaps between previous current samples and the current samples extracted when there is external force on the manipulator.

In summary, both the WMA and the function of the variable span improve estimation performance in terms of response time and estimation quality. The noise in the measured current is reduced by the WMA and thus the diagonal elements of related \({\varvec{\sigma}}_{{\rm {Dyn}}}^{2}\) are reduced. Smaller diagonal elements of covariance matrix \({\varvec{\sigma}}_{{\rm {Dyn}}}^{2}\) lead to a lower response time for the system.

5.2 Influence of the generalized momentum model

Since the dynamic model is based on the format of the generalized momentum, the proposed contact force estimation method works independently of inversion of the manipulator inertia matrix, and consequently the computational burden is low. Furthermore, joint acceleration is not required with the proposed method, and therefore amplification of the measurement noise can be avoided. In particular, acceleration is computed from measured joint angles. However, measurement noise is amplified via the numerical differentiation utilized in a latter step. Uncertainty of the manipulator dynamics is overcome in the approach presented since the noise in the measured current is taken into account and ultimately allows for definition of a more accurate dynamic model. For robotic manipulator tasks that require contact force information, the presented estimation method and control approach are validated in terms of response time and estimation quality. In addition, the costs of physical implementation of the approach cost are lower than the similar standard Kalman filter–based approaches [40, 41] as it works independently of force/torque sensors. It can potentially be applied to any robotic manipulators where the kinematic and dynamic information is accessible. However, the benefits in robotic manipulators of different orientations and sizes have not been established.

5.3 Limitation of the proposed method and future works

It must be noted that in order to apply the proposed estimation method, a time gap between every two occurrences of the external force during the experiment is assumed to be larger than \(t_{s} \cdot t\). \(t_{s} \cdot t\) is a tiny gap and the assumption is valid in many manufacturing applications. However, if the robotic manipulator is used in an environment with high-frequency vibration, such as turning or milling processes, the time gaps between contact may become smaller than \(t_{s} \cdot t\), and the approach may not yield benefit. Future research should be conducted to evaluate the performance of the approach in such turning or milling (or similar) applications and if necessary consider additions to the SKFW calculation that retain the benefits of the proposed approaches.

The proposed approach is validated in a physical system implementation with differing loading scenarios. The validation used key parameters that are critical for the effective implementation of robotic manipulators—response time and force estimation precision. The control system is not aware of the force contact before occurrence nor the weight of the end effector loading. Using a series of loading scenarios shows that parameters of the approach are not specifically tuned to succeed in limited range of applications. Hence, the successful results from the implementation of the approach can be assumed for industrial applications.

Identification results vary as the environment changes. For example, the friction and noise are affected by the loading. This problem was somewhat mitigated in this case since the maximum load on the manipulator was 2 kg. However, a comprehensive system identification method will help to obtain more accurate model parameters. Therefore, modern machine learning techniques will be employed in our future work to determine the friction and noise.

In the case of some particular orientations, singularity may occur and thus, the magnitude of the external wrench in some directions cannot be observed by the robot. Consequently, the effectiveness of the proposed method will be affected. This phenomenon will be considered in the future to improve the generalizability of our work.

6 Conclusion

This paper proposes a novel sensorless contact force estimation method. The serial manipulator is modelled in the format of generalized momentum and the dynamic parameters are identified without additional force/torque sensors. The noisy motor current measurements are optimized by the WMA and the covariance matrix of the predicted state noise is determined with an optimal span \(t\). The measurement noise matrix is obtained taking the uncertainty in the joint speeds into account. The standard Kalman filter is tuned and then used to conduct contact force estimation with the optimized current from the WMA. Experiments are then performed on the UR5 manipulator. Compared to the existing methods in the literature, the proposed estimation approach improves the estimation results.