An accurate dynamic model is the paramount element of any model-based collision detection algorithm. The manipulator dynamics equation generally used is

where q, q., q¨∈ℝn are the vectors of generalized coordinates, velocities, and acceleration, respectively; M(q) ∈ ℝ^n×n is an inertia matrix; C(q,q.)∈ℝn×n is a matrix of Coriolis and centrifugal forces; F(q.)∈ℝn and g(q) ∈ ℝⁿ are the vectors of friction and gravitational torques; τ is the vector of the actuation torques; τ_ext is the vector of the torques induced at the joints by the contact forces, in the absence of contact/collision with the environment τ_ext = 0 (Siciliano et al., 2010). The model requires the exact knowledge of the kinematic and dynamic parameters. In real life, kinematic parameters are known (because of manufacturing standards and calibration of robots before shipping) and provided by manufacturers. However, the latter usually do not provide dynamic parameters, with rare exceptions like, for example, Universal Robots that provide CAD models. Even with the CAD model the torque prediction lacks accuracy because it contains neither friction nor motor inertia parameters. A more accurate model can be obtained by performing identification. In this subsection, we provide an outline of the identification procedure.

The process of the identification of the dynamic parameters of mechanical systems can be divided into seven main steps (Swevers et al., 2007):

Identification starts by deriving the dynamic model of a manipulator moving in free space (τ_ext = 0) in a linear regressor form

where Y(q,q.,q¨)∈ℝn×nπ is the regressor matrix, that can be obtained symbolically or numerically using recursive Lagrange-Euler formulation (Siciliano et al., 2010), modified recursive Newton-Euler formulation (Atkeson et al., 1986), or screw theory formulation (Garofalo et al., 2013), and π∈ℝnπ is the vector of dynamic parameters (standard parameters). It consists of inertial parameters of each link modeled as a rigid body, an actuator, and friction parameters of each joint. The inertial parameters of a link include mass m_i, the first moment of inertia h_i = m_ir_i (the product of a mass and the position of the center of mass), and inertia tensor with respect to the origin of the link I_i. Friction parameters are the coefficient of the Coloumb friction f_c, the coefficient of the viscous friction f_v, and an offset due to motor current offset f_o. Actuator parameter is reflected rotor inertia J_i [If servomotor inertia I_{m, i} and the gear ratio of the transmission k_ri are known, then reflected rotor inertia can be computed as Ji=Im,i/kri2 and the components of the regressor corresponding to reflected inertia should be moved to the right-hand side of Equation (2)]. In total, the vector of dynamic parameters for each link is

Remark. More advanced friction models, for example, models with the Stribeck effect are not linear in parameters, yield a non-linear regressor matrix, that significantly complicates the identification process. However, if the linear friction model does not accurately describe friction torques, then it can be replaced by a non-linear model after identifying all other parameters. The non-linear model is usually identified for each link separately using non-linear least squares (Gaz et al., 2018).

Not all the dynamic parameters enter the dynamic equation of the robot independently; some of them do not enter at all, some of them enter in linear combinations with other parameters. Due to that, the regressor matrix Y(·) has zero or linearly dependent columns leading to singularity which causes problems during trajectory planning and parameter estimation. Several methods have been proposed to overcome these limitations by deriving a set of identifiable parameters called base parameters π_b and corresponding base regressor matrix Y_b(·). Gautier and Khalil (1990) proposed an analytical method based on the recursive properties of robot energy. However, a year later Gautier (1991) proposed numerical methods based on QR- and SVD- decompositions. The analytical method was derived using Denavit–Hartenberg convention and it employs heuristics for determining linear dependence or independence of actuator parameters. Numerical methods (especially QR) are easy to implement and not limited to a specific convention, thus they should be the first choice when approaching parameter reduction.

The choice of trajectory significantly affects the accuracy of identification. For example, random point-to-point motion will most probably not lead to accurate parameter estimates because of the lack of property called persistence of excitation (Anderson, 1982). To find a persistently exciting trajectory it is necessary to carry out trajectory planning that is usually posed as an optimization problem (non-linear program) that aims to find q*(t), q.*(t), and q¨*(t) such that chosen objective function associated with persistence of excitation is minimized. The most commonly used objective functions are condition number of the observation matrix [cond(W_b), for the definition of W_b see Equation (5)] or log-determinant of the moment matrix (log(det(WbTWb))) (Hollerbach et al., 2016). For industrial manipulators, several families of trajectories were proposed in the literature: fifth-order polynomials (Atkeson et al., 1986), truncated Fourier series (Swevers et al., 1997), and their combination (Wu et al., 2012). Fifth order polynomials have fewer parameters to optimize and can guarantee zero velocity and acceleration in the beginning and at the end of the trajectory. Periodic trajectories have an advantage in terms of data processing, as the same periodic trajectory can be executed several times and the collected data can be averaged. Moreover, they allow for exact frequency domain differentiation. However, there is a disadvantage in terms of abrupt change in initial velocities and accelerations (q.i(0)≠0, q.i(T)≠0, q¨i(0)≠0, q¨i(T)≠0), which may cause robot vibration as well as hinder accurate trajectory tracking. Even in cases when zero initial and final velocities and accelerations are imposed as constraints of the optimization problem, the solver struggles to satisfy the constraints. The addition of a fifth-order polynomial solves this problem leading to the trajectory in the form

where w_f is the fundamental frequency, N is the number of harmonics, and ⌊ ⌋ is floor function.

Once trajectory planning has been performed, it is necessary to execute the trajectory on a real robot in closed-loop and record currents/torques, generalized positions q(t), and, if available, generalized velocities q.(t). After the trajectory execution, the recorded data should be processed to remove measurement noise and to reconstruct missing variables from the available ones. If q.(t) are measurable, then only accelerations have to be estimated from velocities; otherwise, velocities have to be found first based on position measurements. As data processing is performed offline, the central difference scheme should be used for a more accurate estimation of derivatives (Hoffman and Frankel, 2018). However, derivative estimates are noisy even with a central difference scheme, so they should be filtered. To avoid introducing delays, non-casual filters (a zero-phase filter) should be used during filtering (Mitra and Kuo, 2006).

For parameter estimation, it is necessary to calculate observation matrix W_b(·) and observation vector T from the collected and processed data as

then, compute base parameters estimates employing ordinary least squares

which minimizes the squared torque prediction error, i.e., the cost function J=12‖T-Wbπb‖2. However, Equation (5) does not guarantee the physical consistency of parameters, thus the properties of the dynamic model (Siciliano et al., 2010). Since model-based techniques are extensively used in robotics, and the properties of dynamic equations are used to prove theorems and to guarantee convergence of controllers and observers, several important developments have been made in dynamic parameter identification in terms of the physical consistency of parameters. First, Sousa and Cortesão (2014) proposed to impose linear matrix inequality constraint on kinetic energy, more specifically—on the generalized inertia matrix of each link (semi-physical consistency)

where S(·) is the operator that maps a 3 × 1 vector into a 3 × 3 skew-symmetric matrix. Then, Wensing et al. (2017) and Sousa and Cortesao (2019) proposed to impose triangle inequality as the linear matrix inequality constraint (physical consistency)

Besides the constraints on inertial parameters of links, it is possible to impose constraints on other dynamic parameters: reflected inertia (J_i > 0) and friction (fvi>0, fci>0). To impose physical or semi–physical consistency constraints while searching for base parameters, Sousa and Cortesão (2014) proposed a bijective mapping from the base and dependent parameters to standard parameters. It does not require additional computation, rather uses the result of base parameter calculation.

For some robots, instead of torque measurements, current measurements are available. In such cases, in addition to π (π_b), it is necessary to estimate drive gains. For that, Gautier and Briot (2014) proposed to carry out an additional experiment with a load attached to the end-effector of the robot and use the extended observation vector T and matrix W_be

where I_u and I_l are current measurements for unloaded and loaded cases respectively; K is a vector of drive gains; Wl=[WluWlk]∈ℝ10 is the observation matrix of the load, divided into parts corresponding to unknown and known inertial parameters. Using Equation (9), it is possible to derive the dynamics equation in a regression form linear with respect to unknown parameters

that allows estimating base dynamic parameters, unknown load inertial parameters, and drive gains, satisfying the (semi)-physical consistency constraints provided that some of the parameters of the load π_lk are known (at least one), usually it is mass because it is easier to measure compared with other inertial parameters. Gautier and Briot (2014) proposed another method for computing drive gains using total least squares to avoid bias errors caused by the same q(t), q¨(t), and q¨(t) used in both parts of Equation (10). However, it is not clear how to impose constraints in that case; without constraints, such a computation method can result in negative drive gains.

Remark. For drive gain identification, it is important to choose a load such that torques for loaded and unloaded trajectories differ. It can be achieved by choosing a heavy load and attaching it in a way that is not aligned with the axis of the end-effector.

After the identification of parameters, model validation should be performed to assess the accuracy of torque prediction for any arbitrary trajectory executed by the robot. One example of a metric that can be used to measure accuracy is root mean square for the prediction error. If torque predictions are poor, then steps 3 to 6 should be repeated until the accurate model of the robot is obtained.

Ideally, to implement collision detection, it is necessary to estimate τ_ext, as it is the only indicator of the collision. However, even with the most recent model identification methods, it is impossible to reconstruct the exact model of a robot (Equation 1). The most accurate model available is

with a mismatch between the real model and its estimate (unmodeled dynamics) being equal to

Using Equation (12) it is possible to rewrite Equation (11) as

Thus, the parameter that can be estimated is lumped disturbance τ_d = −τ_um + τ_ext. If variables q, q., q¨, τ can be measured, then the vector of disturbance torques can be found from Equation (13) through straightforward subtraction. In practice, this approach is hardly applicable, since acceleration measurements are not available, and estimating them from noisy velocity measurements through numerical differentiation further amplifies noise. Therefore, more sophisticated techniques should be considered that would allow for estimating τ_d in absence of acceleration measurements. In the following two subsections we review several disturbance observers proposed in the context of pHRI.

From Equation (13) we know that disturbance observers estimate the sum of torques due to unmodeled dynamics and interaction. An easy and effective way to detect collisions in presence of unmodeled dynamics is to set a static threshold (upper and lower bounds on unmodeled dynamics). The value of the threshold can be chosen as a percentage from maximum torque, for example, ±0.1τ_max (Haddadin et al., 2008) or determined from data used for validation of dynamic parameters. However, inaccurate parameter identification or use of dynamic parameters from CAD may result in a high threshold yielding long collision detection time. To deal with it several solutions were proposed in the literature.

Hu and Xiong (2017) proposed to improve the dynamic model by training a neural network (multi-layer perceptron) to predict the residual between measured torques and torques found from a rigid body model. As the input of the network, they used generalized positions and velocities that, according to authors, provide the same degree of accuracy as the network with generalized positions, velocities, and accelerations. Rigid body model enhanced with neural network allowed authors to choose tight thresholds thus decrease collision detection time.

Ho and Song (2013) proposed using a 2nd order band-pass filter in Equation (33) instead of a low-pass filter (Equation 31). By carefully choosing cut-off frequencies they were able to filter out low-frequency torques due to the motion of the robot and high-frequency torques due to noise, leaving torques caused by the collision. Band-pass filtering allowed authors to decrease threshold 5-fold compared to classical momentum observer yielding to the detection of collision in 5–6 ms. However, their approach is limited to cases when the dynamics of collision is significantly faster than the dynamics of the task, moreover, band-pass filtering modifies estimated disturbance torques making them impossible to use, for example, in collision localization. Haddadin et al. (2008) and Li et al. (2019) proposed to use two observers, one to detect slow or soft collisions using low-pass filtered collision torques and one to detect fast collisions using band-pass filtered collision torques.

Sotoudehnejad et al. (2012) proposed using time-variant thresholds that take into account uncertainties in inertial parameters of the robot as well as friction parameters. To estimate uncertainties authors proposed to conduct time-consuming experiments with a robot applying various collision torque at different states. Although it sounds promising, the difference in the detection time between the time-variant threshold and constant threshold is of the order of 0.1–0.01 s. Briquet-Kerestedjian et al. (2019) proposed to also consider uncertainties due to numerical differentiation used to find generalized velocities and acceleration from noisy encoder measurements. Their approach is different from Sotoudehnejad et al. (2012) in a way they treat the model of the manipulator. Briquet-Kerestedjian et al. (2019) consider a decentralized linear model and treats non-linear coupling between joints as a disturbance. It allows authors to use linear estimation techniques such as Kalman filter. Sotoudehnejad et al. (2012) on the other hand consider a centralized non-linear model of the manipulator and use the momentum observer. In general, it is not clear if time-consuming experiments worth the amount of time gained in collision detection time and if the time-variant threshold is robust and provide consistent result in the whole workspace of the robot.

Within the scope of this work, we developed a C++ library for processing the measurements and estimating disturbance torques (https://github.com/mikhel1984/ext_observer). The library consists of two basic abstract classes: robot and observer. The former provides an interface for defining the dynamic model of a robot. The latter allows working with an observer regardless of its internal implementation: it defines an obligatory method that takes current joint angles, velocities, torques, and time step, and returns the disturbance torque estimate. The library can be included in the source code of a robot control program, used with MATLAB (loaded as a C shared library) or robot operating system (ROS) as a standalone component.

To start working with the library, a user has to create a new robot instance inheriting the basic abstract class and define its dynamic model. The dynamic model can be supplied in two forms: functions for M^(q), C^(q,q˙), F^(q.), and g^(q) generated based on symbolically derived model; numerical procedure such as recursive Newton-Euler algorithm (RNEA) implemented as rneag(q,q.,q¨) where subscript g indicates the value of the gravity constant. If the dynamics of the robot is computed numerically, the problem arises related to the computation of C^(q,q˙)T used in the majority of the observers. One way to overcome this problem is to use the modified Newton-Euler algorithm (De Luca and Ferrajoli, 2009), another is to use the numerical approximation of the time derivative of the inertia matrix (M^.≈ΔM^/Δt). Even though the second method is easier to implement, its accuracy depends on sampling time Δt. We propose a simple way to improve the accuracy, by using the definition of the rate of change of the inertia matrix

where ∂M^i/∂qi≈ΔM^/Δqi has to be calculated numerically, for example, with finite differences. Further, noting that all the observers we have discussed utilize the product C^T and q. rather than C^T, it is possible to reduce computational costs by evaluating the product directly as M^.q.-C^q. or

where p0=rnea0(q,0,q.) and n is the number of joints. From a computational point of view Equation (37) is cheaper than computing M^. and C^ separately because it allows avoiding multiple calls of rnea_g(·) for column-wise evaluation of the matrix M^..

Each observer has to be initialized when called for the first time; we suggest initializing based on the assumption that there is no collision, and disturbance torques are equal to zero. Moreover, each observer depends on its previous states that can be implicit as in the case of filters, or explicit and implemented via the integration of some parameters. Therefore, observer does not work as a “pure function” and cannot be used several times during a time instant.

Universal Robots provides a description of the UR10e in the universal robot description file (URDF). The file contains kinematic parameters of the robot as well as inertial parameters of links but does not contain inertial parameters of the motors, drive gains, and friction parameters needed for accurate modeling of dynamics. Thus, we performed model identification, following the main steps from section 2.1. First, we symbolically derived regressor matrix Y(·) ∈ ℝ^{6 ×84} using kinematic parameters from URDF and Euler-Lagrange formulation of dynamics. Then reduced parameter space using QR—decomposition obtaining πb∈ℝ40 and Yb(·)∈ℝ6×40.

In experiment design, as a trajectory, we chose the combination of truncated Fourier series and fifth-order polynomials (Equation 4); as an objective function—condition number of the observation matrix. The optimization problem was posed in MATLAB and solved using the patternsearch algorithm. We performed trajectory optimization for different periods T (20, 30, 40, 50 s) and number of harmonics N (5, 8, 10, 12, 15); the best estimation in terms of root mean square error of torque prediction was obtained for N = 12 and T = 30 (Figure 1).

The optimized trajectory was executed on UR10e in the velocity control mode. There are several methods to program the robot to execute the desired trajectory: MATLAB, ROS, UR Script. We used UR Script because it allows sending commands to the robot with a higher sampling rate than other methods. Despite the trajectory tracking capabilities of the UR10e, the nominal and real trajectories do not always coincide because of safety limitations on velocities and accelerations. To overcome that either safety constraints can be removed, or maximum velocity and acceleration constraints can be decreased during the trajectory planning phase.

In the data processing stage currents, positions, and velocities were filtered with zero-phase fifth-order Butterworth filter (the filtfilt function in MATLAB). For acceleration estimation, we used numerical differentiation—central difference scheme—followed by zero-phase filtering to remove the noise.

Parameter estimation was divided into two parts: first, we identified drive gains on one trajectory (T = 50, N = 14), then on a different trajectory (T = 30, N = 12) the vector of base and standard parameters. In both cases, physical consistency was imposed by linear matrix inequality constraints using the YALMIP toolbox (Lofberg, 2004). Finally, the parameters were validated on a different harmonic trajectory (Figure 2) for which the root mean square error of torque prediction is rms = [2.814.161.900.700.650.46] Nm. The estimated parameters are shown in Tables 1, 2.

In this subsection we provide the general guideline for selecting and tuning the disturbances observer discussed in the section 2.2. All the observers in absence of collisions show non-zero disturbance torques because of the inaccurate estimates of some dynamic parameters or unmodeled dynamics. The closer identified parameters are to real ones, the smaller are going to be estimated disturbance torques and vice versa. It emphasizes the significance of the accurate dynamic model in estimating torques associated with collisions. For high level comparison of the observers see Table 3.

For collision detection we did two experiments: in one experiment the robot experienced a soft collision with a human, while in another test—a hard collision with a brick. In both cases, manufacturer safety presets were set to the least restricted to achieve fast motions. Model-based estimated disturbance torques were compared with those estimated based on the real and target currents of the UR10e that were computed as

where K is the identified vector of drive gain coefficients, I_target and I_real are the vectors of measured and target currents, respectively.

Due to the modeling error and torque/current measurements noise, the estimated disturbance torques do not equal zero during nominal operation conditions (without collision). To detect a collision it is necessary to set upper and lower thresholds on estimated disturbance torque in the absence of collisions τ^d,ub and τ^d,lb, respectively. When a collision occurs, some of τ^d,i significantly increases depending on the location of a collision crossing corresponding τ^d,ub,i or τ^d,lb,i. The static threshold is the easiest approach to detecting collisions; it can be chosen for a specific task or the whole workspace (any task). On the one side, task-specific bounds for a single or several tasks are tighter and result in faster collision detection time. On the other side, if a robot is reprogrammed to execute a new task, then the threshold can be violated in the absence of collision. Thus, for robots that are constantly reprogrammed to execute different tasks, global bounds are more advantageous even though they can be looser and can result in higher detection time.

To find task-specific thresholds it is necessary to execute a task making sure there is no collision, then estimate disturbance torques and choose bounds that exceed maximum and minimum values of τ^d,i by 5–10% (Figure 8). For global bounds, it is possible to use data from validation or execute several trajectories that cover the whole workspace, then perform the procedure similar to the one for task-specific thresholds. In any case, the bounds will depend on the choice of disturbance observer, particularly on the convergence rate: the higher the convergence rate, the higher the bounds and vice versa (Figure 8). However, although for faster convergence rates the bounds are higher, the detection time is shorter, especially for fast collisions (Figure 7).

Both Figures 7, 8 show that collision detection capabilities of the UR10e and model-based collision detection algorithms with β = 30 were identical for hard collisions (Table 4), while for soft collisions UR10e was slightly faster (Table 5). One possible explanation is that the internal control loop of the UR10e is much faster than the rate at which we read generalized coordinates and velocities, and currents which are around 100 Hz. Another explanation is that the convergence rate of the disturbance observer inside the UR10e control system is higher than the convergence rate of the non-linear disturbance observer we use. The behavior of slower disturbance observer with β = 5 is different from the others; for example, in the case of hard collision of the third joint, estimated disturbance torque did not cross thresholds (Figure 7), but overall collision detection time was the same as for β = 30. In the case of soft collision, the observer with β = 5 was the fastest to detect collision (Table 5). In view of this result, the best strategy could be to use several disturbance observers with different convergence rates and static thresholds.

Theoretically, thresholds can be significantly reduced for fast collisions—collisions that have a distinguishingly higher frequency than the motion of robot—by a band-pass filtering, the dynamics of the robot (Ho and Song, 2013; Li et al., 2019). However, in our experiments by applying band-pass filter we were not able to reduce thresholds and to obtain faster collision detection time without getting false positives. A possible explanation is that the trajectory had abrupt changes in the sign of generalized velocities that caused fast changes in the friction torques, therefore the dynamics of the robot had high frequencies.

Another way to reduce the threshold is to use machine learning to predict unmodeled dynamics (Hu and Xiong, 2017). If enough data is provided for training, then neural networks can approximate unmodeled dynamics with a high degree of accuracy. The limitation of this method is that it requires knowledge of the field that engineers may not have, making it suitable for few who are familiar with neural networks.