Dynamics Identification and Contact Force Estimation of Multi DoF Robot Based on Base Frame Force Sensor

External force detection system plays an important role for robots to operate in the human living environment. Force information determines the interaction between the robot and the environment. However, there are many restrictions on the use of force sensors. Many force sensors are required for wide range contact detection. The force sensor-less method that uses current and angle information is difficult to apply for high reduction ratio mechanisms. In this paper, we propose a method for estimating contact force using one force sensor implemented in the base frame. It is necessary to identify the robot dynamics in order to estimate the contact force. Only the base frame force sensor and encoders are used to identify the dynamics. Therefore, it is very easy to apply. In addition, since it is not affected by joint friction, it can be applied to high reduction ratio mechanisms. The proposed contact force estimation method has a sensitivity for the entire robots. Therefore, contact detection is possible no matter where the force is applied to the robot. The effectiveness of the dynamics identification and the contact force estimation is verified experimentally.


Introduction
Many robots that operate in the human living environment, such as collaborative robots and nursing robots, are being researched (1)(2) . The external force detection system plays an important role for the robot to operate in the human living environment (3) . Force information is very useful for recognizing environmental changes and realizing movements adapted to those.
Traditional force detection methods often use force sensors (4) . However, the measurement range of the force sensor is limited. Many sensors are required for wide-range contact detection, and there are many restrictions including cost. On the other hand, it has been developed that force estimation methods without force sensors (5)(6) . In this method, the contact force is estimated from the motor current and the rotation angle. Force information equivalent to that of a force sensor can be obtained, and sensor costs and mounting restrictions are reduced. The estimation accuracy of this method depends on the dynamic model such as motor inertia, torque constant, and friction. A high reduction ratio mechanism is required for powerful operation, but it is difficult to model the friction. This leads to a decrease in estimation accuracy (8) .
In this paper, we propose a contact force estimation method that has a wide detection range and is not affected by joint friction. The proposed method uses one force sensor implemented to the base frame and encoders for each joint. In this study, the contact force is estimated by inverse dynamics calculation. However, as mentioned above, it is difficult to identify the dynamics such as friction. To solve this problem, we use the dynamic formula of the base frame. The motion equation of the base frame is not affected by joint torque. Therefore, it is possible to identify the dynamics that are not affected by joint friction (9) . Furthermore, it is not necessary to measure joint torque during inverse dynamics calculation. Since this method has sensitivity to the entire robot mechanism, it is possible to estimate the contact force other than the work space.
The structure of this paper is as follows. Section 2 describes the model of the 3-link manipulator used in this study. Section 3 and 4 explain proposed dynamics identification method and contact force estimation method. In section 5, experiments are conducted to verify the effectiveness of the proposed method. Finally, section 6 describes the conclusions of this paper.
The relation between the joint velocity vector and end-effector velocity vector is shown as ̇= ̇ Here, the matrix J indicates the Jacobian matrix. X indicates end-effector position. ̇ indicates the joint velocity.

Dynamics
The dynamics is derived by Lagrange's method (9) . It is expressed by the following equation.
Here, subscripts j and b indicate the joint parameter and base frame parameter. M indicates inertia matrix. q indicates generalized coordinates. b indicates gravity term and Centrifugal / Coriolis term. indicates joint torque and includes friction. k indicates number of contact points. indicates contact force. From (8), the joint torque is 0 in the dynamics of the base frame. Therefore, identification that is not affected by joint friction is possible by using the base frame dynamics.

Dynamics Identification
The dynamics parameter identification formula is derived from the dynamics of the base frame. (8) can be expressed as a linear relationship with the parameters related to inertia. This relationship is expressed by the following equation. Here, indicates reglessa matrix and is a function of generalized coordinates such as joint angle, joint velocity, joint acceleration.
indicates unknown physical parameter vector. In this study, has 10 elements related to the inertia moment, the center of gravity, and the mass.
is expressed as following equation.
1 , 2 , 3 indicate inertia related to gravity. indicates total mass of the robot. In the identification test, the robot is operated under the condition that there is no contact force. The total external force is measured by the force sensor mounted on the base frame, and the joint angles are measured by the encoders. From the relationship in (9), is identified using the measured values of force and joint angles.

Contact Force Estimation
Contact force is estimated using the identified parameters. If there is no contact with the manipulator, the total external force can be measured with the base frame force sensor. Therefore, the external force calculated from the identified model is equal to the external force measured by the force sensor. On the other hand, when the manipulators are in contact, there is a difference in contact force. Therefore, the contact force can be calculated by the following equation.
Here, indicates contact force vector. ̂ indicates identified physical parameter vector.
indicates force measured by base frame.
indicates Jacobian matrix for contact point. In (11), the Jacobian matrix is required to estimate the contact force. However, since the Xw and Yw directions are orthogonal, the Jacobian is an identity matrix. Therefore, the forces in the Xw and Yw directions are uniquely obtained. This means that the contact force can be estimated no matter where the manipulator comes into contact. However, the contact position is required to estimate the contact moment. The contact position can be calculated from the relationship between the moment and the force (10) . It is calculated by the following equation.
Here, , indicate contact position, , , , , indicate base frame force and moment. Since (12) contains an inverse matrix, it may diverge at a singular point. Therefore, in this study, the contact point is not estimated, but only the contact force in the Xw and Yw directions is estimated.

Experimental setup
Experiments on dynamics identification and contact force estimation were conducted. A 3-link manipulator with   A-1KN, Kyowa, Tokyo, Japan) for comparison. Table 1 and Table 2 shows the parameters of the experimental system and specification of force sensors.

Dynamics Identification
The motion trajectory in which the acceleration is excited was input to the robot, and the parameters were identified. The identification test was performed for 30 seconds 10 times. Measurement data of 3000 samples were acquired in one test, and the parameters were identified by the least square method.
The identified results are shown in Tables 3 to 5. The mean and standard deviation of the results of 10 estimations are shown. From the experimental results, it was confirmed that the standard deviation was smaller than the average value. The parameters with a large value had a small standard deviation. This is because the smaller the value of the parameter, the smaller the effect on the force of the base frame. Another factor is that the resolution was poor for the parameters to be estimated. It can be said that the parameters with less influence are the parameters that are not dominant. Therefore, there should be no problem in estimating the contact force.
In addition, the force of the base frame was estimated from the identified model and compared with the measured values. The result is shown in Fig. 3. Table 6 shows the results of root mean square error and correlation coefficient.
From the results, the force of the base frame can be estimated accurately using the identified model.

Contact Force Estimation
The contact force was estimated using the identified model. Six patterns of experiments were conducted with different contact forces. Six patterns were tested by changing the direction and the link to which the contact force was applied. Each pattern is shown in Table 7 and   almost matched. Also, it was possible to estimate in the same way regardless of which link and which direction the contact force was applied. It was confirmed that the entire manipulator has sensitivity. There was an error depending on the pattern. This is because the contact force deviated from the axial direction. It was difficult to apply contact force straight to the axes. Figure 11 shows the results of the Xw-direction estimates of pattern 6. The accuracy can be further improved by considering the coordinate axis directions and contact points as described above.

Conclusion
In this paper, we proposed a method to estimate the contact force using the force sensor implemented to the base frame. A dynamic model was identified for contact force estimation. This identification method uses the dynamics of the base frame. This identification method is not affected by joint friction and does not require measurement of joint torque. Therefore, it can also be applied to robots with a high reduction ratio mechanism. The contact force was estimated from the identified model by inverse dynamics calculation. Due to the orthogonality of the Xw and Yw axes, the contact force can be detected no matter where the manipulator comes into contact. It was confirmed by the experiment that the estimation of contact force was achieved. As future work, verification of estimation accuracy when the robot operates will be performed. It is also expected to be applied to robots that operate in the human living environment.