Collision Detection and Identification on Robot Manipulators Based on Vibration Analysis

Robot manipulators should be able to quickly detect collisions to limit damage due to physical contact. Traditional model-based detection methods in robotics are mainly concentrated on the difference between the estimated and actual applied torque. In this paper, a model independent collision detection method is presented, based on the vibration features generated by collisions. Firstly, the natural frequencies and vibration modal features of the manipulator under collisions are extracted with illustrative examples. Then, a peak frequency based method is developed for the estimation of the vibration modal along the manipulator structure. The vibration modal features are utilized for the construction and training of the artificial neural network for the collision detection task. Furthermore, the proposed networks also generate the location and direction information about contact. The experimental results show the validity of the collision detection and identification scheme, and that it can achieve considerable accuracy.


Introduction
Industrial robots play an important role in the modern manufacturing industry, and humanfriendly robots will soon become flexible and versatile coworkers in the industrial setting [1]. One of the core problems in human-robot interaction is the detection of collisions between robots and the industrial environment, including humans and other manufacturing structures. Indeed, industrial robots should be able to operate in very dynamic, unstructured, and partially unknown environments, sharing the workspace with the human user, and preventing upcoming and undesired collisions [2].
Furthermore, researchers are getting more interested in gathering the maximum amount of information from the impact event, such as the contact position, direction and intensity, in order to let the robot react in the most appropriate fashion. Related concepts include collision avoidance [3], and collision isolation, identification, classification and reaction [2]. Particularly, the concept collision isolation aims at localizing the contact point, or at least which link out of the n-body robot collided. Furthermore, collision identification is to determine the directional information of the generalized collision force [2].
Different approaches for detection of robot collisions have been presented in the literature. A first intuitive approach is to monitor the current transient in robot electric drives, looking for shock changes within currents caused by collisions [4][5][6][7]. A second approach is based on the tactile sensors laying inside robot skins. The more common approach is the so-called model-based method (the state observer or Kalman filter method), and the detection algorithms are mainly based on the evaluation

Vibration Modeling and Feature Extraction
This section address the vibration presentation and its characteristics. First, the modeling method of vibration response is proposed with dynamic equations and transfer functions. Then, the typical features are analyzed based on the mathematical model, together with illustrative examples.

Vibration Response under Collision and Its Mathematical Modeling
In this paper, our research focuses on the vibration response of multiple test points along manipulator structure under several experiments. An effective method for dealing with vibration of kinematic chain mechanism is the elastodynamic modeling and analysis [10,11]. In this method, each critical mechanical structure and component is simplified with stiffness, viscous and mass parameter. As for a n−dof manipulator, it is composed of a series of motors, gears, links and joints, some of which will generate deformation and vibration under collision. For this reason, the dominant vibration structures can be considered as elastic bodies with certain stiffness and viscous coefficients.
We consider a n−dof manipulator with m dominant vibration structures. Its axis displacement vector can be defined as: where q D = [q 1 , q 2 , · · · , q m ] T is the vibration deviation, and q M = [q m+1 , q m+2 , · · · , q m+n ] T denotes the joint displacements. Furthermore, we assume the equilibrium points of q D is q D = [q 1 , · · · , q m ] T . The dynamic equation of manipulator can be written as: the variable τ M , τ f and τ D denotes the joint torque generated by motor, friction and structure deformation respectively. We denote τ D = [τ 1 , · · · , τ m ] T , and τ M = [τ m+1 , · · · , τ m+n ] T . The subscript D denotes the dominant vibration structures, and M denotes the drive motor of manipulator. On the other hand, the torque generated by the deformation of vibration structures can be given by: where K p = diag(k p1 , k p2 , · · · , k pm ), K v = diag(k v1 , k v2 , · · · , k vm ) is the vibration dynamic coefficient matrix. Furthermore, k pi , k vi denotes the stiffness and vicious coefficient of ith dominant vibration structure respectively. We assume that the gravity and friction is compensated by feedback control loop, and the Coriolis and centrifugal effect generated by structure deformation is relatively small. In most cases, the displacement of q D is much smaller than q M , and the inertia matrix M(q) is mainly determined by joint displacement. Furthermore, the torque vector generated by collision is assumed as f ext . Then we get the dynamic function of manipulator as follows where J D T and J M T is the associated geometric contact Jacobian matrix to m vibration structures and n robot motors, respectively. However, the collision torque vector f ext and contact Jacobian J D T , J M T is typically unknown.
, y 1 = q D − q D , and x 2 = q M q M , y 2 =q M , an n inputs and m + n outputs state-space equation is obtained where u is the active motor torque, and f ext is torque vector generated by collision, and the related parameter matrix is given by: Then we get the modal analyzed transfer function from collision torque f ext to vibration deformation y 1 as follows We considered the structure of system matrix A 1 and A 2 , and we got that rank(A 1 ) = 2m and rank(A 2 ) = n. Let λ Di , λ Di (i = 1, 2, · · · , m) be 2m eigenvalues of A 1 , and λ 0 (= 0), λ Mi (i = 1, 2, · · · , n) be the n + 1 eigenvalues of A 2 .
From the partial fraction expansion, the modal analyzed transfer function P(s) becomes By modal analysis, P(s) can be expressed as a linear sum of m + n vibration modes, m modes of which are generated from the m dominant vibration structures. Furthermore, the other n poles which come from A 2 , correspond to the n-dof active dynamic of manipulator.
The matrix Φ k corresponds to the rigid body vibration mode and is positive semidefinite, and it has the following structure For any φ kji ∈ Φ ··i , the subscript k ∈ (1, 2, · · · , l) denotes the serial number of vibration test position, j ∈ (1, 2, · · · , n) denotes the serial number of collision torque in vector f ext , and i ∈ (1, 2, · · · , m) is the number of vibration modes.
We considered the structure of system matrix mainly determined by inertia matrix M(q M ), vibration dynamic coefficient matrix K p and K v , of which the latter two remain nearly unchanged. That means the natural frequencies ω Di (i = 1, 2, · · · , m) will vary with joint displacement q M . As the stiffness coefficients k pi (i = 1, 2, · · · , m) of manipulator structure are usually considerably large, there exist several eigenvalues of A 1 relatively larger than that of A 2 , which comes from the active dynamic of robot and reflects the normal working frequency of manipulator. For this reason, there exist several natural frequencies of vibration that are obviously bigger than the frequency band of active dynamic of robot, i.e., ∃ω Di (i = 1, 2, · · · , m), and ω Di > ω Mj (j = 1, 2, · · · , n) hold.
So we get an important conclusion: the dominant natural frequency of vibration under collisions is independent of the dynamic property of robot. When the inertia matrix is given, the natural frequency under collision should remain the same during different dynamic processes or static statuses.
Furthermore, the modal matrix Φ k depends on the value of geometric contact Jacobian matrix J D T and J M T . For this reason, the contact position information can be induced by the value of vibration mode matrix Φ k .

Vibration Features Analysis and Illustrative Example
This section illustrates the characteristics of vibration modal shape of manipulator under collisions with example. The test data were collected on the STR6-05 robot arm (see Figure 1), a 6-DOF heavy-load manipulator. Four consecutive collision experiments were conducted while the end-effector moved in line; the collision conditions are listed in Table 1. Near end-effector X Human hand The joint displacement and motor current signals of joint 1, 2 and 3 are shown in Figure 2. We label the contact time of corresponding collision events on the figure. We find that there are some shock changes in the motor currents but little change in joint displacements. However, it is hard to detect collision events from current signals directly because of dynamic change and the noise involved in signals, particular for heavy-load manipulator. As shown in the figure, we cannot find some obvious features for collision 1. For vibration signal measuring, the 1A113E and 1A114A industrial accelerometer (made by Donghua Testing Technology) with NI-9232 data acquisition module is used. The 1A113E accelerometer (uniaxial) is mounted beside joint 2, perpendicular to the direction of joint 1 and joint 2. The 1A114A accelerometer (triaxial) is equipped beside the end-effector of manipulator.
The vibration acceleration and vibration modal is shown in Figure 3. The first signal comes from 1A113A located beside joint 2, and the latter comes from 1A114A beside end-effector, corresponding to two perpendicular directions. The frequency characteristic of each accelerometer within sliding windows is shown in Figure 3. There are four obvious frequency charts corresponding to four collisions in each vibration signal. The peak frequencies, i.e., the natural frequencies of three accelerations corresponding to one collision are approximately the same, with different energy densities. The magnitudes of all the peak frequencies constitute the modal matrix Φ in Equation (8). Figure 3 shows that the vibration modes of different parts along robot structure have the following characteristics: 1. The contact position information can be analyzed with vibration modes of different sensors.
The main vibration frequency of nearer sensors is usually higher than sensors far from contact position. That is because manipulator structure can be seen as a low-pass filter, the higher frequency vibration signal is reduced during the propagation process. For example, the magnitude of the 3rd collision in the 1st sensor is comparatively higher than that in the 2nd sensor signals.
That is because the 3rd collision is conducted near the base, which is much nearer to the 1st signal. 2. Similarly, the contact direction can also be determined with vibration modes. The relative energy density value of different directions in the triaxial accelerometer can be used to detect the contact direction. Generally speaking, the vibration of collision direction may have the comparatively higher energy density. For example, the 1st and 2nd collision took place at the same part with different directions, the 2nd magnitude is higher than the 1st one in the 2nd signal while the 2nd magnitude is smaller than the 1st one in the 3rd signal, as is shown in the figure. 3. Furthermore, contact material information can also be reflected by vibration modal. The band width of the 4th collision is much lower than the other 3. That is because the frequency band of human hand contact force is comparatively narrow.
Limiting the range of magnitude below 1 dB, we got the frequency characteristic of normal dynamic comparative to collisions in Figure 4. It shows that the natural frequency of active dynamic appears at low frequency segment (always below 50 Hz), while the vibration frequency by collision appears at intermediate segment. That means the eigenvalues of A 2 is much smaller than that of A 1 in Equation (5), and the dynamics of normal operation do not affect the natural frequencies of collisions. Obviously, the natural frequencies and modal is mainly dependent on the inertia matrix, and independent of the robot dynamic. This means that the detection algorithm can be designed without considering the dynamic property of the robot, and the training and test samples for the model independent method can be collected in some simple or static scenarios.
Since collision will generally cause shock vibration, and its natural frequencies are mainly contained in the high-frequency domain, several symptom parameters in the frequency domain can be selected to represent the collision event. Extracting collision information from frequency domain signal requires proper understanding of the process. For frequency features, such as natural frequencies, the vibration modal shown in the spectrum often has direct or indirect connection to certain dynamic events.

Collision Detection and Identification Method Based on Vibration Features
Based on the vibration features discussed above, we proposed a learning-based algorithm for the detection, isolation and identification of collisions.
The proposed method mainly contains three parts: vibration mode analysis, collision detection, and collision identification. As shown in Figure 5, the vibration signals are first recorded by accelerometer sensors. The vibration mode related features are then extracted from the vibration signals. Any change in the vibration mode from normal condition can indicate the occurrence of collision. Three different Back Propagation neural networks are developed for the detection of collisions (BP1), the isolation of contact part (BP2), and identification of direction (BP3). BP2 and BP3 should be activated only if some collisions are detected by BP1, and the input of BP3 varies with the output of BP2.

Data Processing Collision Feature Extraction Collision Classification
Detection Implement Figure 5. Vibration signal based detection framework.

Vibration Mode Estimation
The main objective of vibration mode estimation is to determine the main natural frequencies ω Di (i = 1, 2, · · · , m), and the values of vibration mode corresponding to each natural frequency φ kji ∈ Φ ··i , where k denotes the serial number of sensors, j denotes the serial number of collision force, and i ∈ (1, 2, · · · , m) denotes the number of vibration modes.
In the collision experiment, the result of measurement is strings of acceleration values in discrete moments. The acceleration signal should be properly processed in order to build the collision classifier. For the discrete string a1(k), a2(k), and a3(k), fast Fourier transform (FFT) is used to determine vibration spectrum within a sliding window which has lower computational cost and better accuracy performance. In our research, the sampling rate is 3.2 k Hz, and the width of sliding window is 320 samples with 50% overlap. Furthermore, the cycle time of detection algorithm is 0.1 s.
For the spectrum of each sampling window, we proposed a peak frequency-based method to determine the natural frequency ω Di (i = 1, 2, · · · , m), the estimation of ω Di . For each, the proposed approach consists of the following steps: Step 1: For acceleration signal k ∈ (1, 2, · · · , l), set a tolerance error ∆ and δ for spectrum analysis; Step 2: Find all the local maximum power and local minimum power from start frequency f 0 to cut-off frequency f e , such that there is no other power value larger than current maximum value between the two adjacent local minimum powers, and there is no other power value less than current minimum value between the two adjacent local minimum power densities; The difference between adjacent maximum and minimum power density should be larger than ∆; Step 3: Collect all the local maximum power values and corresponding frequencies; Step 4: Calculate Num, the number of local maximum power density; Step 5: If Num > m, then ∆ = ∆ − δ, repeat step 2 to step 4; Step 6: Record current local maximum power values P ki (i = 1, 2, · · · , m) and corresponding frequencies f ki (i = 1, 2, · · · , m); Step 7: For other acceleration signal k ∈ (1, 2, · · · , l), repeat step 1 to step 6; Step 8: Rank all the frequency values f ki (k = 1, 2, · · · , l; i = 1, 2, · · · , m), and divide them into m groups with maximum intervals.
With the estimation of modal frequencies, the vibration modal matrix Φ can be determined by extracting the magnitude of corresponding frequency in the spectrum chart, as it shows in Figure 6. In this figure, we get 6 natural frequencies ω D1 , ω D2 , · · · , ω D6 from the measurements of three acceleration signals in collision experiment j. The magnitude values at each characteristic frequency are the estimation of vibration modal. Obviously, there is little error of estimation, e.g., the estimation φ 2j5 . However, this error has limited influence on the final detection result because this detection is based on the synthesis of multiple vibration modal. In this way, we get the estimation of vibration modal matrix of experiment j:

Proposed Artificial Neural Network
In our method, 3 BP networks are implemented for the collision detection, positioning and direction identification respectively. The collision detection artificial neural network together with the modal analysis algorithm should be executed within each sliding window. Once a collision event is detected, the collision positioning artificial neural network is launched with the current vibration modal data. In addition, the input of the 3rd network is dependent on the output of collision position information. The operation process of relevant algorithms is displayed in Figure 7. B-P ANN consists of an input layer, hidden layers, and an output layer of neurons. A neuron serves as a processing unit in which output is a linear or nonlinear transformation of its inputs. The neurons, as a group, serve to map the input vibration modal features to the desired collision patterns. The structure of BP network is shown in Figure 8.
The output signal of hidden layer and output layer can be described in the following equations: where m j (t) is the output of current neurons, w ij is the weight of the connection between current layer neurons and its input layer neurons, b j is the bias of the jth neuron of current layer. The activation function of output layer is linear function, while the hidden layer uses sigmoid function Considering the characteristic of vibration modes discussed in Section 2, we selected the most important features for each kind of detection task, as shown in Tables 2-4.
As the natural frequencies are mainly dependent on inertia matrix M qM , we selected the displacement of joint 2 and joint 3 as input features of detection. The displacements of other joints have little influence on inertia matrix.

Features Function Equation
Geometric Appearance q M2 , q M3 vibration Frequencies ω Di (i = 1, 2, · · · , m) vibration modal φ kji (k = 1, 2, · · · , l; j = 1, 2, · · · , n; i = 1, 2, · · · , m) The contact position and direction of collision can affect the comparative vibration magnitude of sensor on different positions and directions. We select relative modal between test position for the positioning of contact, and relative modal between test direction beside contact position for direction identification. Table 3. The features used for the isolation of contact position.

Experiment Dataset Preparation and Training Procedure
The experiment dataset is collected on the platform in Figure 1. The robot is controlled with PD control law during the experiment. An uniaxial accelerometer is mounted beside joint 2, perpendicular to the direction of joint 1 and joint 2. In addition, a triaxial accelerometer is equipped beside the end-effector of manipulator. All acceleration data is recorded by NI-9232 data acquisition module. Our experiments are conducted with an aluminum impact hammer, which is also connected to NI-9232 data acquisition module. In this way, the collision time and force data is recorded, as shown in Figure 9. The collision experiments are performed with eight selected contact points, with different direction and force intensity. As the natural frequencies of vibration are mainly dependent on inertial matrix and joint displacement (see Section 2.1), we select five typical working patterns (see Figure 10) as testing standard. Our collision experiments are performed while the robot is transforming randomly from one pattern to another. Hundreds of collision experiments are performed on the platform. The vibration modal data of acceleration signals during these experiments are analyzed with the sliding-window method introduced above. Typical vibration modal data sets with corresponding experiment conditions are collected for classification research. Meanwhile, we also collect the vibration modal data during collision-free operations. We get in total 800 experiment samples, of which 50 percent are collision samples performed on 8 different contact positions along robot structure. Eighty-five percent of these samples are selected randomly for training and the remaining are used for testing.

Detection and Identification Results
In this subsection, several test divisions of experiment data are utilized to evaluate the efficiency of the proposed method. Our BP-ANN algorithms are taken from the Neural Network toolbox in Matlab. At the training stage, we optimize the weights and bias parameters by minimizing mean squared error, according to Levenberg-Marquardt optimization.
For the detection of collisions, BP networks with different architecture are constructed and trained. Table 5 lists the accuracy of detection ANN of different architectures (The structure i − j − k − o means a network with i input neurons and o output neurons, and j and k stands for the number of neurons in hidden layers ). It is clear to see from the table that the proposed method has considerable accuracy for collision detection, and an average accuracy of 0.95 can be obtained with 1 or 2 hidden layers. All the vibration modal data of actual collision are used for the training and testing of positioning neural networks. Considering the geometric layout of the STR6-05 robot, the moveable structure is divided as two parts, i.e., one part is link 3, and the other contains link 4, link 5 and link 6, as shown in Figure 10. Table 6 lists the accuracies of different layers of positioning networks. In addition, the BP networks with 2 hidden layers are suitable for the positioning task. As the vibration modal features of collision direction are mainly reflected by the acceleration signals nearby the collision point, the input of direction identification network is determined by the output of positioning network. Table 7 lists the accuracy of network of 3 architectures. The training and testing samples come from the vibration data of collisions on link 4, link 5 and link 6, which is near to the acceleration sensors on end-effector. It is obvious that the BP network with 2 hidden layers can obtain comparative stabilization accuracy. Considering all the results comprehensively, the proposed method can be utilised for the detection, positioning and identification of collisions with considerable accuracy. By analyzing, we find that the positioning error and identification error is mainly derived from boundary samples, that is the collisions near joint 3. One more accelerometer located beside joint 3 may be used for the enhancement of accuracy.
By analysing the training and test procedure, we can find that an experiment of 300 collision samples is enough for the artificial network training for any 6 − do f manipulator with different sensor placement scheme, and it can be accomplished in half or one hour with well designed scenarios and test procedures.

Rapid Prototyping System Design
In order to realize the online test of the proposed method, the computation complexity and detection time of the related algorithms should be estimated. As discussed above, the main calculation consumption includes three parts, namely, the fast Fourier transform of vibration acceleration data, the estimation of natural frequency and modal, and the node outputs update of neural network.
It can be seen from Section 2.2 that the collision vibration frequency of the robot is mainly between 50 Hz and 1500 Hz, and we set the sampling rate to 3200 samples/second. On the other hand, in order to ensure the spectral characteristics have a sufficient resolution, we select a sliding window of 0.1 s for each cycle, and the number of sampling points N = 320 for Fourier transform, as shown in Figure 11. The overlap amount of the sliding window is 50 percent, that is, the calculation cycle of the proposed algorithm is 0.05 s. Within each computing period, Butterfly fast Fourier transform is adopted, and the total number of real multiplications required by FFT of N points is 2N * log 2 (N), and the total number of real numbers added is 2N * log 2 (N) for each vibration signal. For the natural frequency and modal estimation algorithm, the main computational cost of the algorithm is the sorting algorithm of spectrum amplitude, and the computational complexity of the algorithm is O(N * log 2 (N)) [26], that means, the computational cost of the frequency and modal estimation algorithm can be ignored relative to the fast Fourier transform.
For the status update of the neural network, each neuron includes multiple addition operations, multiplication operations and exponential operations, and the exponential operation can be converted into a number of addition and multiplication operations. For the collision detection neural network with typical structure of 27-6-4-1, the addition calculation times of a single update is 278, and the total number of multiplication is 98. For the positioning and direction identification neural network with one or two hidden layers, the calculation cost is of the same magnitude. Therefore, the main computational cost of this method is derived from the fast Fourier transform. In addition, within a single update cycle (50 ms), the addition and multiplication times of the algorithm is about 22,000 and 21,000 respectively. The total computation consumption of this method is about the same size as the traditional state observer method with Newton-Euler Function, as discussed in [27].
Based on the above analysis, we use Simulink/Data Acquisition Toolbox to develop a rapid prototyping system, as shown in Figure 12. The vibration data within each sliding window is buffered, and then used for FFT, modal estimated, and neural network status updates. The system is a slower-than-real-time system due to data caching and operating system. It shows that the collision detection time is about 0.1 s, and the isolation and identification time will be a little longer.

Conclusions and Discussion
In this work, we present a model independent method for robot collision detection, positioning and identification. The vibration signals are analyzed and used for the construction and training of BP neural network. The test results of the experiment confirm that it is possible to build a monitoring algorithm with considerable accuracy. The conclusions may be summarized as follows: 1. With a small amount of training samples (about 300-500 samples), the proposed method can provide considerably high accuracy, and it can be conducted in half an hour or one hour with any kind of manipulators. Therefore, this method has the potential to be implemented in real application scenarios. 2. The detection and identification method is mainly dependent on the frequency domain features of collision, the time-domain features can also be added to improve detection accuracy and computational efficiency in further research. 3. The bandwidth of collisions on heavy load manipulator is mainly below 1500 Hz, and the bandwidth of light robot should be much less than that value. This means some high-performance MEMS accelerometers may be utilised on some occasions. 4. The main calculation consumption of the proposed method comes from the FFT of vibration signal, some dedicated FFT chips can be utilized to improve detection performance.

Conflicts of Interest:
The authors declare no conflict of interest.