External force estimation for robotic manipulator base on particle swarm optimization

The safe disposal of nuclear waste in radioactive environment urgently needs cost-effective approaches. Toward this goal, this article developed a method to external force estimation based on the identified model without force sensors. Firstly, the mathematical model including joint friction was obtained and transformed into the linear combination of unknown parameter to be estimated. Secondly, the unknown parameters were identified based on the improved particle swarm optimization algorithm, the identification procedure was implemented by optimizing the excitation trajectories to excite joint motion and sampling relevant data. Identified results were compared with the biogeography-based optimization algorithm and the cuckoo search algorithm. Then, the identified dynamic parameter was applied to external force estimation. Finally, the verification of external force estimation has been carried out using the Kinova Jaco2 robot manipulator, and the experimental results showed that the external forces by the proposed method could be estimated with an root mean square error of 0.7 N.


Introduction
The safe disposal of nuclear waste is a global problem of great social significance and the global nuclear industry increasingly seeks cost-effective approaches for remote handling techniques to reduce radiation hazard. Remote robot manipulators can take the place of workers to do too hazardous or difficult tasks in radioactive environment where the use of people is not always possible. Many tasks performed in disposal of nuclear waste require remote robot manipulators to interact with the environment, such as grasping, cutting, dismantling, sorting, and segregating nuclear waste. To ensure the accurate execution of these tasks, not only the position/velocity control but the force control is also required to obtain the desired performances. 1 Therefore, it is important to take the information of interaction forces into account for achieving safe and desired task performance. To obtain the external force information, a force sensor is used; the force sensor is often installed on the wrist of robot manipulator for measuring the external forces applied on the tip of the end effector. However, the force sensor with high accuracy and reliability is very expensive and undoubtedly increases the system cost of the robotic manipulator. Technically, the uncertain disturbances and harsh environment, such as high temperature and the high dose rates of nuclear sites, may affect the accuracy and reliability of the force sensor. Moreover, the use of force sensor not only increase the complexity of mechanical, electrical, and software design of the robot system but also have an influence on system modeling and control algorithm design. 2 Considering the abovementioned drawbacks of force sensor, employing the external force estimation method has been a feasible option in radioactive environment where the dose rates limit the use of force sensor.
The external force estimation methods developed to estimate the forces applied on robot end effector require parameter identification of robot for an accurate mathematical model. Different parameter identification methods for the robot model have been suggested. Swevers et al. 3 presented an approach to design the optimized excitation trajectory, this approach was based on a finite Fourier series and guarantees periodic excitation, which allows data averaging in time domain and rejects the negative effects of noise disturbance. Atkeson et al. 4 developed the least square algorithm to estimate load and link inertial parameters of a PUMA 600 industrial robot. Wang et al. 5 built the Adaline neural network model to estimate the dynamic parameter for the reconfigurable robot. Taking into account the measurement errors, Olsen and Petersen 6 presented the maximum likelihood estimation (MLE) for estimating inertial parameter. This scheme can reject the influence of measurement noise remarkably, but it is easy to get stuck at locally optimal result in the global. Jin and Gans 7 proposed an intuitive and efficient criterion using Hadamard's inequality to design the excitation trajectory for dynamic parameter identification. The reliability of the identified parameters has been verified on a six-degree-of-freedom (6-DOF) Staubli TX-90 robot. Wang et al. 8 employed deep learning to eliminate the effect of uncertainty to aid parameter estimation. Gautier et al. 9 proposed a closed-loop output error approach called the Direct and Inverse Dynamic Identification Models (DIDIM) for offline identification of the dynamic parameters, which only need to collect the joint torque measurement information, instead of calculating the joint velocity and acceleration by the filtered value of position measurement. In recent years, there are several novel methods for identifying the robot dynamic parameter using intelligence algorithms. Xu and Ke 10 presented an biogeography-based optimization (BBO) method to estimate the dynamic parameters by searching for global optimal solution. Ding et al. 11 developed an artificial bee colony method to identify the unknown parameters of a 6-DOF industrial robot, which employed the measured joint position to derive the joint velocity, acceleration and combined it with the motor current to calculate the joint torque. Ding et al. 12 proposed an improved cuckoo search (CS) method to obtain the parameter of ER-16 robot manipulator, while it can be easily trapped into local optimum when solving complex multidimensional parameter identification problems. Jia et al. 13 developed optimization design method based on multicriteria embedded by optimized excitation trajectories, and the parameter identification is solved by MLE.
On the other hand, various methods for estimating the interaction between the robot and its environment have been proposed. A robust force estimation observer using semiparametric model and disturbance Kalman filter is proposed to estimate the contact force of multilink manipulator. 14 A high-order finite-time observer is proposed to estimate the time-varying and nonconstant interaction forces in a finite time horizon. 15 An external force observer-based admittance method with time-varying delay compensation is developed for hydraulic series elastic actuators. 16 Sensorless contact force estimation method based on the weighted moving average with variable span is developed for serial manipulator. 17 Disturbance observer-based method is proposed to estimate the unknown interaction force. 18,19 Interaction force estimation that utilizes extended state observer was presented to estimate the interaction time-varying forces exerted on a robotic manipulator. 20 However, the above method may suffer from decreased estimation accuracy due to model errors.
Motivated by the need to perform tasks in disposal of nuclear waste in radioactive environment, we proposed a method to estimate the external forces applied on robot end effector without force sensors. The main contributions of this article are listed as follows: (a) Compared with two different intelligence algorithms, the superiority of the particle swarm optimization (PSO) algorithm in the application of parameter identification is validated. (b) A new method to estimate the external forces without force sensor based on the identified model together with the built-in torque sensor. (c) Experimental results carried on the Kinova Jaco 2 robot manipulator verify the effectiveness of the proposed method.
The remaining part of this article is organized as follows. The linear dynamical model of robot manipulator with friction is described in the second section. Parameters identification based on the PSO algorithm with excitation trajectory design and optimization is elaborated in the third section. In the fourth section, the identification procedure as well as experimental validation of external force estimation was implemented on the 6-DOF Kinova Jaco 2 robot manipulator. Additionally, the performance of the PSO algorithm is compared with some other intelligence algorithms. Finally, a brief conclusion is given in the final section.

Dynamic modeling
Applying the recursive Newton-Euler 21 or the Lagrange formulation, 22 the dynamic model of a robot manipulator consisted of n rigid links connected by n rotational joints can be formulated as where D ðq Þ refers to the symmetric inertial matrix, C ðq ; _ q Þ denotes the nonlinear Coriolis and centrifugal force vector, and G ðq Þ denotes the gravity loading force vector; q , _ q , and € q refer to the joint angle vector, velocity vector, and acceleration vector, respectively; t d is the actuator torque acting on joints; t di is the actuator torque of each link i.
To effectively identify the dynamic parameters, equation (1) can rearrange into the linear combination of identifiable parameters by the modified Newton-Euler method 23 where F d ðq ; _ q ; € q Þ denotes the (n Â 10n) identification matrix, which can be treated as a nonlinear function depending only on the motion data; q d denotes the unknown dynamic parameters. The dynamical parameters of link i contained 10 parameters are given by where l xxi ; l xyi ; l xzi ; l yyi ; l xzi ; l zzi À Á is the inertial tensor of link i, m i r xi ; m i r yi ; m i r zi À Á denotes the first mass moments of link i; m i is the mass of each link i. Noting that m i is contained within G ðq Þ in equation (1), and other components of dynamical parameters of link i except for m i are contained within D ðq Þ and C ðq ; _ q Þ in equation (1). Due to the friction comprising a large proportion of the joint output torque, the torques caused by joint friction should be also included in dynamic model of robots in equation (3). To simplify the friction model, the robot joint friction can be regarded as the linear form of viscous friction and Coulomb friction in robotic applications. 24 The friction torques are described as where t f denotes the vector of joint friction torques; f c is the Coulomb coefficients vector; f v is the viscous friction coefficients vector. Furthermore, in the presence of the external torques, the total joint torque vector can be expressed as where t ext denotes the external torque on joints generated by the external force/torques, the integrated dynamic model can be rewritten as a linear parameterized form in the absence of external force/torques, which depends upon a set of parameters to be identified. 25 where t refers to joint actuator torques, F ðq ; _ q ; € q Þ denotes an (n Â 12n) identification matrix as described in equation (1), and q is the (12n Â 1) unknown inertial parameters vector. Thus, the unknown parameters of link i can be rewritten as q i ¼ ½q di ; f ci ; f vi T . Indeed, the identification matrix Φ in equation (8) is not full rank, it means only part of the inertial parameters have made a dynamic contribution. Therefore, the inertial parameters to be estimated can be simplified to the minimal set of inertial parameters with m identifiable parameters, that is, the vector of base parameters. It can be obtained by the numerical QR decomposition (QRD) and singular value decomposition (SVD). 26 Equation (8) can be rewritten as where F m ðq ; _ q ; € q Þ denotes the n Â (m þ 2n) vector identification matrix. q m represents the unknown dynamic parameters with (m þ 2n) vector, which contain the vector of base parameters and the friction parameters.
Once the dynamic parameters vector q m was estimated by collecting the measured position and joint torque. The external torque on joints can be estimated by employing equation (3), which results from the reorganization of equations (1) and equation (7) t The estimated external force/torques on the robot end effector in Cartesian space can be calculated by (11) where J T denotes the transpose of the robot's Jacobian matrix. F ext is defined as follows The estimated external force/torques on the robot end effector are obtained by multiplying the inverse of J T (13) Parameters identification procedure

PSO for parameters identification
PSO is a heuristic optimization algorithm, which was inspired from birds' flocking for exploring global optimal solution. 27 PSO can be applied to estimate the dynamic parameters by initializing a set of particles randomly over the searching space to find the optimal solution with iteration. Particles update themselves by tracking two "extremum" in each iteration. The first one is individual extremes (p Best ), and the another one is global extremes (g Best ) found by the whole PSO. Figure 1 shows the illustration of the PSO algorithm to obtain the optimal solution, the particles are searching in the n-dimensional solution space, where each particle adjusts its velocity and new position in accordance with its own and companions' experience.
The velocity and position of the particles are updated in each iteration according to the following equations x j ðt þ 1Þ ¼x j ðtÞ þṽ j ðt þ 1Þ (15) wherex j ðtÞ andṽ j ðtÞ stand for separately the position of the particle and the speed of the particle, respectively;x j ðt þ 1Þ andṽ j ðt þ 1Þ are the particle position and the velocity for the next moment, respectively; coefficients C 1 and C 2 are the positive constant; R 1 ; R 2 2 ½0; 1 are independently random number. o is the inertia weight coefficient in a range of.[0,1] The inertia weight coefficient o in equation (14) can be expressed by o(t þ 1) ¼ ao(t) (0 < a < 1) which decrease linearly with the increase of iteration. This coefficient enables the particle swarm to search in the whole solution space with high probability at the initial stage and quickly converge to the local optimal solution. Then, the optimal solution can be searched in the local detail with the decrease of inertia weight.
To avoid the PSO algorithm falling into the local optimum, the mutation operation is carried out on the particles by referring to the concept of mutation in the genetic algorithm where x jk , y jk represent the k-dimensional element of particle j, r k is a random number in the range of the k-dimensional element, randðÞ is a random number in the range of [0,1]; R 2 (0,1) denote the mutation probability. The fitness function determines whether the particle is the optimal condition in the PSO algorithm. In this work, the fitness function fitðx j ðtÞÞ is defined as where N is the length of the measured data, t xi (x ¼ 1, 2, . . . , n) is the actual torque of the joints that need to be identified. Similarly, t pxi (x ¼ 1, 2 . . . , n) denotes the predicted torque according to equation (9). d is a weighting coefficient in the range of [0,1]. The goal of the search is to get a minimum of the function. Based on PSO for estimation of dynamical parameter, 12 parameters should be identified for each joint in the dynamic model of the manipulator, the detailed procedure for executing the PSO algorithm to estimate the dynamic parameters is described as follows: Step 1: Randomly initializing the particle swarm. Initializing the swarm size according to the number of parameters needed to identify, randomly initializing the particle position and velocity in the range of kinetic parameters, initialize p Best and g Best .
Step 3: Doing the following for each particle: Step 3.1: Obtaining the variant particle swarm y ðtÞ according to equation (16).
Step 3.2: Selecting the favorable variation and carrying out the variation on the particle swarm xðtÞ: Step 3.3: Carrying out fitness comparison between each individual in the current particle swarm and p Best according to equation (17), and update p Best .
Step 4: Comparing the fitness of p Best for each particle, and update g Best . Step 5: Checking the stop criterion. If the stop criterion is not matched, go to Step 2, otherwise, go to Step 6.
Step 6: The particle corresponding to the global extreme value is the optimal solution of the population, that is, the identified parameters of the robot manipulator.
The detailed PSO algorithm procedure for parameter identification is depicted in Figure 2.

Excitation trajectory design
To decrease the influence of measurement noise in estimating q m from equation (9), the identification matrix F m ðq ; _ q ; € q Þ should be well-conditioned. The movement of the robot manipulator along the designed excitation trajectory needs to ensure the sufficiency of excitation. Several trajectory parameterization methods have been applied to the parameter identification of robot manipulator, such as the interpolated trajectory 28 and the fifth-order polynomials. 29 We employed a finite Fourier series as the exciting trajectory in this work, for improving the signal-to-noise ratio of the measurement and reducing the effect on robot flexibility. 30 The trajectories for joint i are parameterized as follows where q i;0 denotes the constant offset term, ! f denotes the fundamental frequency of excitation trajectories; N is the number of harmonics and exciting trajectory for each joint contains 2N þ 1 parameters. The coefficients a i;k , b i;k are the amplitude of the trigonometric functions. Convergence rate of the parameter estimation and the noise rejection depend on the condition number of identification matrix, and it should be ensured that the objective function of optimization is to minimize the number of conditions. Thus, the condition number of identification matrix is taken as the fitness function in PSO algorithm, and the initial population is designed according to motion constraints of robot joints. The excitation trajectory optimization with the motion constraint can be described as 11 minðcondðF ÞÞ where F is the identification matrix, q imin , q imax , _ q imin , _ q imax , and € q imin , € q imax are the lower and upper constraints of the joint position, velocity, and acceleration on each joints, respectively. ðqðtÞ f grepresents the set trajectories of robot end effector, O denotes the workspace of the robot manipulator. The excitation trajectory optimization could be solved using optimization toolbox in MATLAB.

Experimental results
The identification procedure was implemented on the 6-DOF Kinova Jaco 2 robot manipulator as shown in Figure 3. This robot manipulator has six rotation joints, including two base joints, one elbow joint, and three wrist joints. The joints of the robot manipulator have speed limitations at 48 s À1 . The position encoders and the built-in torque sensors are fixed on all joints, all of six joints can provide the joint torque as well as the measured information of joint position, velocity while working. The classic Denavit-Hartenberg (DH) parameters for the 6-DOF Kinova Jaco 2 robot manipulator are given in Table 1. Figure 4 shows the optimized excitation trajectories with the base frequency of 0.1 Hz. The excitation trajectories are the parameterized five-term Fourier series given  in equation (19), containing 11 optimum parameters for each Fourier series. All the optimum parameters are presented in Appendix 1.
The optimal excitation trajectories q i ðtÞ (i ¼ 1, 2, . . . ,6) for the six joints were controlled by the Kinovarobotics/ matlab_Kinova-api_wrapper for the robot manipulator. The sampling frequency was set to 100 Hz, and the joint torque, position, and velocity data are recorded over 10 periods. It should preprocess the recorded data before identification due to measurement noises. A Savitzky-Golay filter was employed to smooth the raw data of joint position and velocity due to its excellent properties for smooth differentiation, and the conventional low-pass IIR Butterworth filter was chosen to filter the measured and averaged joint torques owing to no differentiation is needed. The end-effector path in Cartesian for Kinova Jaco 2 manipulator during the optimal trajectory is shown in Figure 5.
To highlight the performance of the proposed method, comparison with the BBO method 10 and the CS algorithm 12 is performed for the same excitation trajectory. Parameter identification procedure using the above three algorithms is performed in MATLAB 2016b environment. The parameter settings for the three algorithms are given in Table 2.
The max iteration number is set to 100 for all the algorithms and the results of parameter identification are obtained based on 10 independent runs. Figure 6 shows the solving process of the three algorithms, the result of fitness function decreases with the increase of iteration. When  Step size scaling factor a ¼ 0.25 Lévy exponent l ¼ 1.5 PSO: particle swarm optimization; BBO: biogeography-based optimization; CS: cuckoo search.   iterations reach 55, the optimization process of the PSO algorithm tends to be stable and the optimal result of fitness function is 2.7273, whereas those of BBO and CS were 27.8158 and 40.0971, respectively. The final optimization results which are calculated by equation (17) are 0.9293, 1.7815, and 1.9622 corresponding to the PSO, BBO, and CS algorithm, respectively. The results turned out that the PSO algorithm has a faster convergence rate and a better result than the BBO and CS algorithm. The identified dynamic parameters obtained from the PSO algorithm using the optimal excitation trajectories are shown in Table 3. Figure 7 shows the comparison of the estimated joint torque based on the identified parameters for each joint  with the actual torque (obtained with built-in torque sensors). All the estimated joint torque of the three algorithms can track the actual torque in all the joints, which implied that the estimated torques can be remodeled based on the identified parameters and the data of joint motion. It can be also observed that the estimated torques obtained with the PSO algorithm match better with the actual torque than the BBO and the CS algorithm, which highlights that the PSO algorithm has a good performance of global searching and obtained the optimal parameters for robot manipulator. It should be noted that the estimated torque not match well with the actual torque and the estimated torque errors increase during joint velocity reversal. It is mainly due to the bias of parameter estimation, the unmodeled dynamics, and the nonlinear friction which is not included in the model. To quantitatively evaluate the performance of the identified model by using the PSO, BBO, and CS algorithm, the root mean square (RMS) error between the estimated joint torque and the actual torque from tracking the optimal excitation trajectories are compared. The RMS error is defined as 31 where t ei is the estimated torque value of the ith point, t ai is the ith sample of the actual torque. N is the total number of sampling points. The RMS error between the estimated joint torque and the actual torque is compared in Table 4. It can be seen from the results that the identified parameters using the PSO algorithm are more accurate than those of other algorithms.
To validate the efficiency of the identified parameters applying to external force estimation by the proposed method, the elastic impact experiment was carried out. The experimental setup was composed by the Robotiq FT 300 Force Torque Sensor and the spring with elastic constant of 1.12 N/mm, the spring was attached to Robotiq Force Sensor, as shown in Figure 8. It should make sure to keep the posture of the end effector approximately perpendicular to the horizontal plane, so that the spring maintain perpendicular to force sensor as much as possible. The manipulator was commanded to accomplish an up-down movement with the period of 5 s.
The comparison of the actual torques with the estimated torques based on the identified parameters by the three algorithms is shown in Figure 9. The known models can only track the movement induced by inner torque of robot manipulator during the spring compression, and yet the actual torque included the inner torque and external torque acting on the joints. Thus, the external torque can be derived from the difference of the actual torque and the estimated torque. It should be note that the main effort for the movement of end effector was realized by joints 2, 3, and 4 during compression. The external torques exerted on joints shown in Figure 9 were given by equation (10), which considered the external torque as the unknown disturbance to the robot manipulator.
Based on the estimated external torques of joints (see Figure 9), the estimated external forces acting on the robot end effector can be calculated by equation (13). Figure 10 shows estimated external forces and the measured forces (obtained with Robotiq FT 300 Force Torque Sensor) in the Z direction. The difference between the measured force and the estimated force in the Z direction is shown in Figure 11. As can be seen, the external force estimated by the three algorithms has the same trend as the measured force in terms of magnitude and frequency, which revealed the estimated external force has high linearity to the spring response. However, the external force estimated by the PSO algorithm matches better with the measured force in magnitude. The RMS error of the estimated external forces acting  on the robot end effector by the three algorithms is compared in Table 5. From these results we can observe that the RMS error of the external force estimated by the PSO algorithm is smaller than those of other algorithms, which highlights the precision of the external force estimation by the proposed method.

Conclusion
In this article, we developed a new method to estimate the external forces without force sensor based on the identified model together with the built-in torque sensor. The optimal periodic excitation trajectories are designed for parameter identification, and all the unknown parameters for the model of a 6-DOF robot manipulator are identified by the proposed algorithm. The identified results showed that the proposed algorithm has fast convergence rate and higher identification accuracy compared with the BBO algorithm and the CS algorithm. Further, we conducted the elastic impact experiments to confirm the validity of the identified    parameters applying to external force estimation, and the experimental results demonstrated that the proposed external force estimation method can successfully estimate the external force acting on the end effector of the robot manipulator with an RMS error of 0.7 N. This method can be not only a cost-effective alternative to the conventional force sensors but also a feasible solution for redundant force sensing in radioactive environment. In the future, we would focus on the controller design based on the estimated external force.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed the receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Natural Sciences Foundation of China (62071213) and the Scientific Research Fund of Hunan Provincial Education Department (18C0478).