# Research on Collision Point Identification Based on Six-Axis Force/Torque Sensor

Authors: Zhijun Wang; Lu Liu; Wenkai Yan; Jing He; Bingyan Cui; Zhanxian Li
Journal: Journal of Sensors
 (2020)

---

## Abstract

The collision detection algorithm of the robot body previously needed to rely on the surface geometry information of the colliding object and no deformation was allowed during the collision process. To solve this problem, a new robot body collision detection algorithm that uses the force information of the six-axis force/torque sensor at the base to self-constrain is proposed which does not rely on the geometric information of the colliding object surface, and the deformation also allows deformation during the collision. In terms of sensor data preprocessing, a gravity and dynamic force compensation algorithm for the six-axis force/torque sensor at the base is proposed to ensure that the reading of the six-axis force/torque sensor at the base always maintains the value of 0 when the robot is working. Then, the robot is considered to have collided with the outside world when the sensor reading exceeds the set threshold. And a precision factor is proposed to analyze the influence of force and collision distance on the accuracy of the algorithm. Finally, the new algorithm proposed in this paper is compared with the traditional algorithm that relies on the geometric information of the colliding body surface. The experimental results indicate that the accuracy of the collision point detection algorithm proposed in this paper is close to that of the traditional method, but it does not need to rely on the geometric information of the collision body surface, and there is no requirement for whether there is deformation during the contact process. It can be concluded that the collision distance is the most important factor affecting the accuracy of the algorithm, followed by the conclusion of the magnitude of the collision force through the calculation of the precision factor. The results show that this method can effectively detect the collision point of the machine body, and the maximum error at the farthest point of the robot is 8.712%, which lays a certain foundation for the subsequent research on human-machine collaboration in small collaborative robots.


---

## Body

## 1. Introduction

With the development of robot technology, a variety of sensors such as vision, touch, and force were applied to robot systems. Force perception is an important function for intelligent robots to interact with the external environment, especially for operations such as grasping, contour detection, obstacle avoidance, human-computer interaction, and force feedback control [1–3]. The perception of force information also plays a vital role in the hexacopter equipped researched by Ibrahim et al. [4] and the new full-mobile robot and control system researched by Kilin et al. [5]. The perception of force information is conducive to improving the robustness of the hexarotor and fully movable robot control. Six-axis force/torque sensors are widely used in industrial robot collision detection and feedback control [6–8]. In some smarter robot systems, six-axis force/torque sensors are used to implement functions such as tactile sensing, safety control, and collision detection [9–14]. It can be seen that collision detection and feedback control are two key functions of force perception, and collision position detection is one of the important contents of collision detection.There have been many researches on collision detection algorithms using six-axis force/torque sensors in the past few years. Bicchi et al. [15] proposed a solution for contact position detection based on a six-axis force/torque sensor as early as 1990, which allows for practical devices that provide simple, relevant contact information in practical robotic applications. Kazanzides et al. proposed a method for detecting collision points by a linear regression method in a surgical robot system in [16], but did not give a specific calculation process. Zhou et al. proposed a mathematical model for determining the contact position between the fingertip and the object to be grasped/manipulated using measurement data provided by a force/torque sensor installed at the end of the fingertip in 1996, and the errors were analyzed [17]. The real-time selection algorithm described above was used to implement contact point measurement based on a six-axis force/torque sensor. The experimental results show that the test results still have good stability in the case of large noise and interference. Leng et al. [18, 19] used constraint equations and compatibility solution analysis to ensure collision point detection. In addition, gravity compensation and dynamic compensation are described, and the theory is proved to be effective in a gravity environment in experiments. The geometric and natural constraints of the collision process have also been analyzed by many scholars, and the gravity compensation method has also been applied to the collision detection of robots [10, 20].Although the methods mentioned above provide a certain theoretical basis for the detection of robot collision points, there are still obvious shortcomings. The previous method has limited application range due to the reliance on geometric constraints to solve. Therefore, the collision objects must be regular in shape or geometrically modelable (such as a probe with a certain length, sphere, ellipse, and plane), and the collision surface must be smooth and cannot be deformed. The previous method of relying on geometric constraints to calculate the collision point has great limitations because most of the actual collision environment will be accompanied by deformation. In Figure1, the collision types of robots can be divided into three types according to the number of contacts on the collision surface: single-contact collision (Figure 1(a)), flat or surface uniform contact collision (Figure 1(b)), and multicontact collision (Figure 1(c)). However, single-point collisions can be divided into structured collisions (which can model the collision body surface) and unstructured collisions as shown in (e) and (d) in Figure 1. In Figure 1(f), deformation is also an important issue to be considered in the collision process. In fact, collisions with uniform contact on the surface and collisions with multiple contacts can be considered as special combinations of single-contact collisions. Therefore, this article focuses on the analysis of single-contact collision and deformation in an unstructured environment.Figure 1
Collision type classification.In this paper, a robot collision point detection system based on six-axis force/torque sensor is proposed and experimental studies were carried out. We propose an error factor to search for the best in order to improve the detection accuracy. Compared with the traditional method that relies on the geometric information of the colliding body surface, the method proposed not only has no requirements on the surface of the colliding object, but also has good accuracy in the case of shape changes. The algorithm proposed solves the limitations of traditional geometric constraint methods that require strict surface information and do not allow deformation. This paper effectively promotes the application of force perception in robot systems and also lays the foundation for the research of multicontact detection.The remainder of this paper is organized as follows. Collision point detection algorithms and minimum error search strategies are proposed in Section2. Section 3 proposes gravity compensation and dynamic force compensation algorithms. Section 4 conducts experimental research on the proposed collision point detection algorithm and verifies the correctness and accuracy of the algorithm. Finally, the paper is summarized in Section 5.


## 2. Collision Point Detection Algorithm

### 2.1. Mathematical Model for Collision Point Detection

Six-axis force/torque sensor is installed at the base of the robot in order to measure the collision points of the robot body extensively, which is shown in Figure2.Figure 2
Schematic diagram of sensor installation location.Six-axis force/torque sensors can resolve forces and torque into components on three coordinate axes. Therefore, assuming that the collision force and moment areFFx,Fy,Fz and MMxMyMz, the relationship between force and moment and contact position can be expressed as follows:
(1)M=P×F,where PPx,Py,Pz is the vector of the position of the sensor to the contact point. Equation (1) can be expressed in matrix form as follows:
(2)Fy−Fx00Fz−Fy−Fz0Fxxyz=MxMyMZ.The equation system has multiple solutions forF=0, so other constraints are needed to determine the unique contact position.In fact, the solutionP of Eq. (2) is distributed on a spatial straight line when FFx,Fy,Fz and MMxMyMz are constants. This space curve is usually called the external force vector line, which is defined as Lcp. The parameter form can be expressed as follows:
(3)Lcp:xFx=y+Mz/FxFy=z+My/FxFzLcp.The external force vector line is determined by the collision force, andFFx,Fy,Fz is the direction vector of the straight line. Generally, the direction of the collision object’s movement and the direction of the collision force cause two sets of external force vector lines to intersect at different times, and the intersection point is the collision contact point. Therefore, the position of the contact point can be obtained by solving the intersection of the two sets of external force vector lines, which can be expressed as follows:
(4)Px,y,zx,y,z∈Lc1p∩Lc2p.


### 2.2. Projection Method for Collision Points

The external force vector line calculated using the data of two adjacent frames of the sensor under ideal conditions without error is shown asl1 and l2 in Figure 3(a) and intersects at point p1x,y in space. However, the two external force vector lines may not intersect in space due to the measurement error of the sensor, which is shown as l2 and l3 in Figure 3(a). Therefore, a projection method is proposed in this paper to solve the contact coordinates. The two external force vector lines l2 and l3 in space will not intersect at a certain point in space due to the existence of errors, but the projected straight lines on a certain plane will intersect at point p2x,y, as shown in Figure 3(b).Figure 3
Diagram of external force lines in space.
(a)
Two external force vector lines in space(b)
Schematic diagram of space linear projectionThe projection point coordinates of the contact point in three coordinate planes can be obtained by projecting the intersection point, and then the three-dimensional coordinate value of the contact point can be obtained by combining the original external force vector line equation, which is shown in Figure4.Figure 4
Basic principle diagram of projection method.The projection ofLcp on the XOY plane is set to LXOY′, the projection on the XOZ plane is LXOZ′, and the projection on the YOZ plane is LYOZ′. Then, we can get
(5)LXOY′:FyFzx−FxFzy=−FxMx−FyMyLXOZ′:FyFzx−FxFyz=FxMx+FzMzLYOZ′:FxFzy−FxFyz=−FyMy−FzMz.P′x,y is the intersection of the projection lines LXOY,1′ and LXOY,2′, andP1,P2 are the corresponding points of the projection points on Lc1 and Lc2. P1 and P2 can be solved by the following formulas:
(6)ηL1′P′ηL2′P′Lc1pandηL1′P′ηL2′P′Lc2p,where η is a determination factor. η is used to select an optimal projection plane from the three coordinate planes, and the specific rules are as follows:
(7)Ln′=LXOY,n′,maxθXOY,θXOZ,θYOZ=θXOYLn′=LXOZ,n′,maxθXOY,θXOZ,θYOZ=θXOZLn′=LYOZ,n′,maxθXOY,θXOZ,θYOZ=θYOZ,where n=1,2, and θXOY represents the angle between LXOY,1′ and LXOY,2′, which can be calculated with the following formula:
(8)θXOY=Fy1Fx1−Fy2Fx2θXOZ=Fz1Fx1−Fz2Fx2θYOZ=Fy1Fz1−Fy2Fz2.The solving formulas forP1x1,y1,z1 and P2x2,y2,z2 can be expressed as Eq. (9) and Eq. (10) if XOY is the final selected projection plane after the above calculation.
(9)Fy1Fz1−Fx1Fz10Fy2Fz2−Fx2Fz20−Fz10−Fx1xyz1=−Fx1Mx1−Fy1My1−Fx2Mx2−Fy2My2My1,(10)Fy1Fz1−Fx1Fz10Fy2Fz2−Fx2Fz20−Fz20−Fx2xyz2=−Fx1Mx1−Fy1My1−Fx2Mx2−Fy2My2My2.As it is impossible to confirm which point the real collision point is closer to, the contact point is temporarily defined as the midpoint ofP1 and P2, which can be expressed as follows:
(11)P=12P1+P2.


### 2.3. Least Error Search

This paper proposes a method to search the optimal solution among multiple sets of data during the collision process in order to further reduce the error and improve the robustness of contact position detection. Assuming that the external force vector lines generated by multiple sets of force sensor data during the collision are shown in Figure5, the contact set obtained by the projection method using two sets of adjacent data is shown in Figure 6, where the purple point in the point set represents the real contact position, and the orange point is the optimal solution searched by the minimum error method.Figure 5
Multiple sets of external force vector lines during collision.Figure 6
Result of collision point solution.IfP″x″,y″,z″ is the result of the collision point calculation, then ζζMx,ζMy,ζMz is introduced as a precision factor to reduce the calculation error. The Eq. (2) can be expressed as follows:
(12)Fy−Fx00Fz−Fy−Fz0Fxxyz=Mx+ζMxMy+ζMyMZ+ζMz,then
(13)ζMx=Fzy″−Fyz″−MxζMy=Fxz″−Fzx″−MyζMz=Fyx″−Fxy″−Mz.Assumingζ=ζ2Mx+ζ2My+ζ2Mz, the smallest point ζ in the multiple sets of data is the optimal result of collision point detection. Finally, the improved contact position detection method of the system can be summarized as the flow shown in Figure 7.Figure 7
Flow chart of collision point position detection.


## 2.1. Mathematical Model for Collision Point Detection

Six-axis force/torque sensor is installed at the base of the robot in order to measure the collision points of the robot body extensively, which is shown in Figure2.Figure 2
Schematic diagram of sensor installation location.Six-axis force/torque sensors can resolve forces and torque into components on three coordinate axes. Therefore, assuming that the collision force and moment areFFx,Fy,Fz and MMxMyMz, the relationship between force and moment and contact position can be expressed as follows:
(1)M=P×F,where PPx,Py,Pz is the vector of the position of the sensor to the contact point. Equation (1) can be expressed in matrix form as follows:
(2)Fy−Fx00Fz−Fy−Fz0Fxxyz=MxMyMZ.The equation system has multiple solutions forF=0, so other constraints are needed to determine the unique contact position.In fact, the solutionP of Eq. (2) is distributed on a spatial straight line when FFx,Fy,Fz and MMxMyMz are constants. This space curve is usually called the external force vector line, which is defined as Lcp. The parameter form can be expressed as follows:
(3)Lcp:xFx=y+Mz/FxFy=z+My/FxFzLcp.The external force vector line is determined by the collision force, andFFx,Fy,Fz is the direction vector of the straight line. Generally, the direction of the collision object’s movement and the direction of the collision force cause two sets of external force vector lines to intersect at different times, and the intersection point is the collision contact point. Therefore, the position of the contact point can be obtained by solving the intersection of the two sets of external force vector lines, which can be expressed as follows:
(4)Px,y,zx,y,z∈Lc1p∩Lc2p.


## 2.2. Projection Method for Collision Points

The external force vector line calculated using the data of two adjacent frames of the sensor under ideal conditions without error is shown asl1 and l2 in Figure 3(a) and intersects at point p1x,y in space. However, the two external force vector lines may not intersect in space due to the measurement error of the sensor, which is shown as l2 and l3 in Figure 3(a). Therefore, a projection method is proposed in this paper to solve the contact coordinates. The two external force vector lines l2 and l3 in space will not intersect at a certain point in space due to the existence of errors, but the projected straight lines on a certain plane will intersect at point p2x,y, as shown in Figure 3(b).Figure 3
Diagram of external force lines in space.
(a)
Two external force vector lines in space(b)
Schematic diagram of space linear projectionThe projection point coordinates of the contact point in three coordinate planes can be obtained by projecting the intersection point, and then the three-dimensional coordinate value of the contact point can be obtained by combining the original external force vector line equation, which is shown in Figure4.Figure 4
Basic principle diagram of projection method.The projection ofLcp on the XOY plane is set to LXOY′, the projection on the XOZ plane is LXOZ′, and the projection on the YOZ plane is LYOZ′. Then, we can get
(5)LXOY′:FyFzx−FxFzy=−FxMx−FyMyLXOZ′:FyFzx−FxFyz=FxMx+FzMzLYOZ′:FxFzy−FxFyz=−FyMy−FzMz.P′x,y is the intersection of the projection lines LXOY,1′ and LXOY,2′, andP1,P2 are the corresponding points of the projection points on Lc1 and Lc2. P1 and P2 can be solved by the following formulas:
(6)ηL1′P′ηL2′P′Lc1pandηL1′P′ηL2′P′Lc2p,where η is a determination factor. η is used to select an optimal projection plane from the three coordinate planes, and the specific rules are as follows:
(7)Ln′=LXOY,n′,maxθXOY,θXOZ,θYOZ=θXOYLn′=LXOZ,n′,maxθXOY,θXOZ,θYOZ=θXOZLn′=LYOZ,n′,maxθXOY,θXOZ,θYOZ=θYOZ,where n=1,2, and θXOY represents the angle between LXOY,1′ and LXOY,2′, which can be calculated with the following formula:
(8)θXOY=Fy1Fx1−Fy2Fx2θXOZ=Fz1Fx1−Fz2Fx2θYOZ=Fy1Fz1−Fy2Fz2.The solving formulas forP1x1,y1,z1 and P2x2,y2,z2 can be expressed as Eq. (9) and Eq. (10) if XOY is the final selected projection plane after the above calculation.
(9)Fy1Fz1−Fx1Fz10Fy2Fz2−Fx2Fz20−Fz10−Fx1xyz1=−Fx1Mx1−Fy1My1−Fx2Mx2−Fy2My2My1,(10)Fy1Fz1−Fx1Fz10Fy2Fz2−Fx2Fz20−Fz20−Fx2xyz2=−Fx1Mx1−Fy1My1−Fx2Mx2−Fy2My2My2.As it is impossible to confirm which point the real collision point is closer to, the contact point is temporarily defined as the midpoint ofP1 and P2, which can be expressed as follows:
(11)P=12P1+P2.


## 2.3. Least Error Search

This paper proposes a method to search the optimal solution among multiple sets of data during the collision process in order to further reduce the error and improve the robustness of contact position detection. Assuming that the external force vector lines generated by multiple sets of force sensor data during the collision are shown in Figure5, the contact set obtained by the projection method using two sets of adjacent data is shown in Figure 6, where the purple point in the point set represents the real contact position, and the orange point is the optimal solution searched by the minimum error method.Figure 5
Multiple sets of external force vector lines during collision.Figure 6
Result of collision point solution.IfP″x″,y″,z″ is the result of the collision point calculation, then ζζMx,ζMy,ζMz is introduced as a precision factor to reduce the calculation error. The Eq. (2) can be expressed as follows:
(12)Fy−Fx00Fz−Fy−Fz0Fxxyz=Mx+ζMxMy+ζMyMZ+ζMz,then
(13)ζMx=Fzy″−Fyz″−MxζMy=Fxz″−Fzx″−MyζMz=Fyx″−Fxy″−Mz.Assumingζ=ζ2Mx+ζ2My+ζ2Mz, the smallest point ζ in the multiple sets of data is the optimal result of collision point detection. Finally, the improved contact position detection method of the system can be summarized as the flow shown in Figure 7.Figure 7
Flow chart of collision point position detection.


## 3. Dynamic Force Compensation Algorithm

### 3.1. Gravity Compensation Algorithm

The influence of the gravity of the robot body and the end load on the sensor during the movement should be eliminated first when the six-axis force/torque sensor is installed at the base of the robot, and the dynamic force compensation of the sensor should be performed so that the sensor is not changed in the state of robot motion. After dynamic force compensation is performed on the sensor, the reading is constant to zero when the robot is in motion and is not impacted by external forces. It indicates that the robot collided unexpectedly with the outside world, when the sensor reading exceeds the threshold. Robot gravity compensation is shown in Figure8.Figure 8
Gravity analysis.The D-H parameters are used to establish the joint coordinate systemi of the robot. The general formula of the homogeneous transformation matrix ii−1Tof the connecting rod can be expressed as follows:
(14)ii−1T=cosθi−sinθi0ai−1cosαi−1sinθicosαi−1cosθi−sinαi−1−disinαi−1sinαi−1sinθisinαi−1cosθicosαi−1dicosαi−10001.Equation (14) can be expressed as follows:
(15)ii−1T=Rii−1pii−10001,where Pii−1=pxpypzT, Rii−1is the rotation matrix of coordinate system i relative to coordinate system i−1, which can be expressed as follows:
(16)Rii−1=nxoxaxnyoyaynzozaz,where Rii−1is a unit orthogonal matrix, Ri−1i=Rii−1T=Rii−1−1.The pose transformation matrix between adjacent links can be obtained in turn by bringing the parameters of each link of the robot into Eq. (14). The transformation matrix of the coordinate system i relative to the coordinate system {0} can be expressed as follows:
(17)Ti0=T10T21T32⋯Tii−1=Ri0pi001,The position vector of the center of massci of the joint i relative to the joint coordinate system i is as follows:
(18)ci=cixciycizT.If the gravity of each joint of the robot isGi, then the gravity vector of each link can be expressed in the base coordinate system as follows:
(19)Gi=00Gi.The component of the gravity of each joint at its center of mass will change with the robot’s posture. It can be known from Eq. (18) that the rotation matrix of the joint i attachment coordinate system i relative to the base coordinate system is Ri0. The centroid coordinate system ci of the connecting rod i has the same direction as the coordinate system i. Therefore, the gravity vector of each joint on the coordinate system ci can expressed as follows:
(20)Gcii=Gii=Ri0TG0i.The homogeneous transformation matrix of the joint centroid coordinate systemci and the joint coordinate system i can be shown as
(21)Tcii=100cix010ciy001ciz0001.The homogeneous transformation matrix of the centroid coordinate systemci of each link with respect to the {0} system can be expressed as follows:
(22)ci0T=Ti0Tcii=Rci0pci001,where Pci0=pcxpcypczT. The force vector and moment vector of the robot’s own gravity on the {0} system in the stationary state can be expressed as follows:
(23)f0=00∑i=1nGiT,m0=‐∑i=1nGi×pcy‐∑i=1nGi×pcx0T.The force sensor coordinate systemS has the same attitude as the base coordinate system {0} and is offset by -h on the Z-axis; then, we can get the transformation matrix of f0 and m0 in the coordinate system {0} and the coordinate system S which can be expressed as follows:
(24)SfSm=R0S0SpS0R0SR0S0f0m,where
(25)R0S=100010001,(26)SpS0=0−PzpyPz0px−pypx0=0h0h00000.


### 3.2. Dynamic Force Compensation Algorithm

It is obviously not enough to only compensate the sensor for gravity during the robot movement. At this time, the inertial force and the coercive force generated during the movement of the robot need to be considered. Therefore, the robot dynamic force compensation algorithm is proposed in this section.We assume that the mass of each joint arm of the robot ismii=1,2,⋯,6, the position vector of the centroid ci of the joint i with respect to the coordinate system i is rii=1,2,⋯,6, the displacement of joint i is θi , the speed is θ̇i, and the acceleration is θ¨i. Then, we can get
(27)ωi+1=Rii+1ωi+θ̇i+1Zi+1,(28)ω̇i+1=Rii+1ω̇i+Rii+1ωi×θ̇i+1ei+1+θ¨Zi+1,(29)v̇i+1=Rii+1v̇i+ω̇i×pi+1i+ωi×ωi×pi+1i,(30)v̇ci+1=v̇i+1+ω̇i+1×ri+1+ωi+1ωi+1×ri+1,where ωi is the angular velocity of link i, ω̇i is the angular acceleration of link i, v̇i is the linear acceleration of link i, v̇ci is the linear acceleration of the center of mass of link i, and ω0=ω̇0=v0=v̇0=000T, Zii=1,2,⋯,6=001T. The schematic diagram of dynamic force compensation for the sensor is shown in Figure 9. Then, we can get the force/torque relationship between the links, which can be expressed as follows:
(31)Fii−1=Fii+1+miv̇ci−Gcii,(32)Mii−1=Mii+1−ri+1,ci×Fii+1+ri,ci×Fii−1+Iiω̇i+ωi×Iiωi,where Fii−1 is the acting force of rod i‐1 on rod i; Fii+1 is the acting force of rod i+1 on rod i; Mii−1 is the resistance of rod i‐1 to rod i; Mii+1 is the resistance of rod i+1 to rod i; ri,ciis the sagittal diameter from the origin Oi of the coordinate system attached to the joint i to the center of massci; ri+1,ci is the sagittal diameter from the origin Oi+1 of the coordinate system attached to the joint i+1 to the center of massci+1;Ii is the inertia tensor of the rod i with respect to its center of mass ci; ωi×Iiωi is the scientific force term.Figure 9
Link force diagram.Forn-degree-of-freedom robots, the value of Fnn+1is related to the end load of the robot. Fnn+1=0 when the end is unloaded. From Eqs. (27)–(32), F01 and M01 can be calculated. Take them into the Eqs. (24)–(26) to get the specific dynamic force compensation values Sf and SM that are required by the six-axis force/torque sensor at the base. If the force/torque sensor readings of the six-axis force/torque sensor on the base are fDandMD when the robot is in a certain posture, the force/torque information after gravity compensation can be expressed as follows:
(33)FM=fD−SfMD−SM.After dynamic force compensation, the robot can keep the six-axis force/torque sensor reading constant to 0 under the condition of no external force during the movement. Once the reading of the six-axis force/torque sensor at the spindle exceeds the set threshold, the robot can be considered to collide with the outside. The threshold value needs to be determined according to the characteristics of different robot systems. The collision between the robot and the outside world is a continuous process, so each frame of data read by the sensor at the base can be used as input to the collision point detection algorithm after dynamic force compensation.


## 3.1. Gravity Compensation Algorithm

The influence of the gravity of the robot body and the end load on the sensor during the movement should be eliminated first when the six-axis force/torque sensor is installed at the base of the robot, and the dynamic force compensation of the sensor should be performed so that the sensor is not changed in the state of robot motion. After dynamic force compensation is performed on the sensor, the reading is constant to zero when the robot is in motion and is not impacted by external forces. It indicates that the robot collided unexpectedly with the outside world, when the sensor reading exceeds the threshold. Robot gravity compensation is shown in Figure8.Figure 8
Gravity analysis.The D-H parameters are used to establish the joint coordinate systemi of the robot. The general formula of the homogeneous transformation matrix ii−1Tof the connecting rod can be expressed as follows:
(14)ii−1T=cosθi−sinθi0ai−1cosαi−1sinθicosαi−1cosθi−sinαi−1−disinαi−1sinαi−1sinθisinαi−1cosθicosαi−1dicosαi−10001.Equation (14) can be expressed as follows:
(15)ii−1T=Rii−1pii−10001,where Pii−1=pxpypzT, Rii−1is the rotation matrix of coordinate system i relative to coordinate system i−1, which can be expressed as follows:
(16)Rii−1=nxoxaxnyoyaynzozaz,where Rii−1is a unit orthogonal matrix, Ri−1i=Rii−1T=Rii−1−1.The pose transformation matrix between adjacent links can be obtained in turn by bringing the parameters of each link of the robot into Eq. (14). The transformation matrix of the coordinate system i relative to the coordinate system {0} can be expressed as follows:
(17)Ti0=T10T21T32⋯Tii−1=Ri0pi001,The position vector of the center of massci of the joint i relative to the joint coordinate system i is as follows:
(18)ci=cixciycizT.If the gravity of each joint of the robot isGi, then the gravity vector of each link can be expressed in the base coordinate system as follows:
(19)Gi=00Gi.The component of the gravity of each joint at its center of mass will change with the robot’s posture. It can be known from Eq. (18) that the rotation matrix of the joint i attachment coordinate system i relative to the base coordinate system is Ri0. The centroid coordinate system ci of the connecting rod i has the same direction as the coordinate system i. Therefore, the gravity vector of each joint on the coordinate system ci can expressed as follows:
(20)Gcii=Gii=Ri0TG0i.The homogeneous transformation matrix of the joint centroid coordinate systemci and the joint coordinate system i can be shown as
(21)Tcii=100cix010ciy001ciz0001.The homogeneous transformation matrix of the centroid coordinate systemci of each link with respect to the {0} system can be expressed as follows:
(22)ci0T=Ti0Tcii=Rci0pci001,where Pci0=pcxpcypczT. The force vector and moment vector of the robot’s own gravity on the {0} system in the stationary state can be expressed as follows:
(23)f0=00∑i=1nGiT,m0=‐∑i=1nGi×pcy‐∑i=1nGi×pcx0T.The force sensor coordinate systemS has the same attitude as the base coordinate system {0} and is offset by -h on the Z-axis; then, we can get the transformation matrix of f0 and m0 in the coordinate system {0} and the coordinate system S which can be expressed as follows:
(24)SfSm=R0S0SpS0R0SR0S0f0m,where
(25)R0S=100010001,(26)SpS0=0−PzpyPz0px−pypx0=0h0h00000.


## 3.2. Dynamic Force Compensation Algorithm

It is obviously not enough to only compensate the sensor for gravity during the robot movement. At this time, the inertial force and the coercive force generated during the movement of the robot need to be considered. Therefore, the robot dynamic force compensation algorithm is proposed in this section.We assume that the mass of each joint arm of the robot ismii=1,2,⋯,6, the position vector of the centroid ci of the joint i with respect to the coordinate system i is rii=1,2,⋯,6, the displacement of joint i is θi , the speed is θ̇i, and the acceleration is θ¨i. Then, we can get
(27)ωi+1=Rii+1ωi+θ̇i+1Zi+1,(28)ω̇i+1=Rii+1ω̇i+Rii+1ωi×θ̇i+1ei+1+θ¨Zi+1,(29)v̇i+1=Rii+1v̇i+ω̇i×pi+1i+ωi×ωi×pi+1i,(30)v̇ci+1=v̇i+1+ω̇i+1×ri+1+ωi+1ωi+1×ri+1,where ωi is the angular velocity of link i, ω̇i is the angular acceleration of link i, v̇i is the linear acceleration of link i, v̇ci is the linear acceleration of the center of mass of link i, and ω0=ω̇0=v0=v̇0=000T, Zii=1,2,⋯,6=001T. The schematic diagram of dynamic force compensation for the sensor is shown in Figure 9. Then, we can get the force/torque relationship between the links, which can be expressed as follows:
(31)Fii−1=Fii+1+miv̇ci−Gcii,(32)Mii−1=Mii+1−ri+1,ci×Fii+1+ri,ci×Fii−1+Iiω̇i+ωi×Iiωi,where Fii−1 is the acting force of rod i‐1 on rod i; Fii+1 is the acting force of rod i+1 on rod i; Mii−1 is the resistance of rod i‐1 to rod i; Mii+1 is the resistance of rod i+1 to rod i; ri,ciis the sagittal diameter from the origin Oi of the coordinate system attached to the joint i to the center of massci; ri+1,ci is the sagittal diameter from the origin Oi+1 of the coordinate system attached to the joint i+1 to the center of massci+1;Ii is the inertia tensor of the rod i with respect to its center of mass ci; ωi×Iiωi is the scientific force term.Figure 9
Link force diagram.Forn-degree-of-freedom robots, the value of Fnn+1is related to the end load of the robot. Fnn+1=0 when the end is unloaded. From Eqs. (27)–(32), F01 and M01 can be calculated. Take them into the Eqs. (24)–(26) to get the specific dynamic force compensation values Sf and SM that are required by the six-axis force/torque sensor at the base. If the force/torque sensor readings of the six-axis force/torque sensor on the base are fDandMD when the robot is in a certain posture, the force/torque information after gravity compensation can be expressed as follows:
(33)FM=fD−SfMD−SM.After dynamic force compensation, the robot can keep the six-axis force/torque sensor reading constant to 0 under the condition of no external force during the movement. Once the reading of the six-axis force/torque sensor at the spindle exceeds the set threshold, the robot can be considered to collide with the outside. The threshold value needs to be determined according to the characteristics of different robot systems. The collision between the robot and the outside world is a continuous process, so each frame of data read by the sensor at the base can be used as input to the collision point detection algorithm after dynamic force compensation.


## 4. Simulation Experiment

### 4.1. Simulation Verification Experiment

A three-degree-of-freedom robot was built to perform simulation experiments to verify the correctness and effectiveness of the algorithm proposed in this paper, as shown in Figure10. The links and joints of the manipulator are center symmetrical to reduce the amount of calculation, and the center of gravity of each link is located on its own central axis. The structural parameters of each link of the three-degree-of-freedom robot used in this paper are shown in Table 1, and the material is set to alloy (ρ=2.7×103kg/m2). In Figure 10, l1=120mm, l2=150mm, l3=200mm, l4=200mm, and h=30mm.Figure 10
Robot structure.Table 1
Connecting rod parameters.
LinkQuality(kg)Center of mass coordinates in each link coordinate system (mm)01.598(0,0,60)11.686(0,0,70.024)22.489(84.685,0,0)33.201(104.684,0,0)The date of six-axis force/torque sensor at the base will change continuously over time. The data collected by the sensor at the base is preprocessed and brought into the proposed collision point detection algorithm. The robot model is imported into ADAMS, and two sets of simulation experiments are designed to verify the proposed dynamic force compensation algorithm and collision point detection algorithm. The magnitude, direction, and position of the collision force applied in these two sets of experiments are known, and the data in the experimental results are based on the sensor coordinate systemOS. The first set of experiments was designed to verify the proposed gravity compensation algorithm and collision point detection algorithm, and an equal amount of increased collision force was applied at the same collision point pc=204.32,68.96,518.46. The robot was set to be stationary in this set of experiments, and θ1=−π/6, θ2=−π/6, θ3=−π/4, and h=30mm. The results of the experiment are shown in Table 2, where the collision direction is obtained by calculating the difference of the coordinate system data of the sensor before and after the force is applied in the ADAMS. Calculation force and calculate position can be obtained by Eq. (24) and Eq. (11) and a series of coordinate transformations.Table 2
Experimental results of different external forces.
NumberCollision force (N)Collision direction (N)Collision position (mm)Calculation force (N)Calculate position (mm)Force direction error (N)Force error (%)Position error (mm)Position error (%)140-32.0642204.32-31.2463217.96820.81791.533013.65827.10220.427168.9619.997674.14600.42955.186012.4347518.4613.2315539.49480.796821.0348260-46.3017204.32-45.2675216.66471.03420.507512.34476.86433.970568.9634.289475.09080.32796.130817.3827518.4618.4023538.52271.019620.0672380-62.6532204.32-60.4687216.35562.18452.661212.03566.54149.334668.9648.675074.48500.65965.52506.3780518.466.1794539.49480.198619.16844100-81.4014204.32-78.6781192.44652.72332.848111.87356.17356.346768.9654.761473.0761.58534.116014.1018518.4615.7934537.13311.691618.67315120-95.9861204.32-98.7495194.14372.76342.187510.17635.68469.780468.9670.645164.86870.86474.091317.8147518.4617.1665500.81080.648217.64926140-114.0861204.32-117.1907214.21273.10461.54649.89275.49380.648368.9679.950965.49770.69743.46238.9562518.469.2236535.94930.267417.48937160-126.5287204.32-124.5523214.25841.97640.43029.93845.20896.167368.9697.404072.96441.23674.004418.5024518.4619.4785533.76120.976115.30128180-143.1024204.32-145.1510211.96382.04861.30907.64384.431105.867168.96106.883374.15961.01625.199626.7182518.4627.5855506.42240.867312.03769200-161.4531204.32-160.3099196.24091.14320.60928.07914.173115.614668.96114.967465.57420.64723.385823.7941518.4624.4413530.42730.647211.967310220-177.0267204.32-174.6555211.11132.37121.20276.79133.675127.346868.96126.273863.83381.09425.126229.0575518.4628.1632509.74170.89438.7183In order to verify the effectiveness and accuracy of the dynamic force compensation algorithm and collision point detection algorithm in the experiment 2, the following assumptions were added on the basis of experiment 1:θ̇1=θ̇2=θ̇3=0; θ¨1=0.3491rad/s2, θ¨2=0.1746rad/s2, and θ¨3=0.4363rad/s2. The robot moves according to the set parameters in experiment 2 and performs the collision test at different points with the same collision force when running to the 3rd second. Specific experimental data is shown in Table 3.Table 3
Experimental results of different collision points.
NumberCollision force (N)Collision direction (N)Collision position (mm)Calculation force (N)Calculate position (mm)Force direction error (N)Force error (%)Position error (mm)Position error (%)110092.687340.0394.562940.46271.87562.20440.43271.547010.348609.67450.04360.67410.043636.0828148.0337.5501149.9261.46731.8960210062.3486060.38131.34681.96732.37991.34681.785673.948338.4972.750037.64331.19830.846725.3831167.0324.3152167.89721.06790.86723100-82.943646.83-84.190547.31611.24691.9460.48612.391548.6725049.60712.37910.93462.3791-27.4107184.16-29.051485.83911.64071.6791410076.894264.0778.761566.71831.86730.75652.64832.854014.687936.8316.419538.99731.73162.1673-62.2218264.19-61.6539261.17630.56793.01375100-16.483482.64-18.050579.17091.56711.52833.46913.967984.67816.7685.66159.40810.98342.6481-50.5759306.82-51.4226313.31370.84676.49376100-45.0167116.79-46.3635122.10081.34680.87605.31084.8264-62.864369.72-62.229776.48910.63466.769163.4159298.7164.4505302.47131.03463.76137100-86.0348190.46-87.7972180.78661.76242.25009.67347.578032.186480.7333.051692.22370.865211.493739.5228422.67-40.6704408.17661.147614.4934810072.1894213.4170.7132226.90011.47620.579613.49017.779230.1609124.0829.6686135.71480.492311.6348-62.2817486.49-63.2758503.80370.994117.31379100-80.7934219.46-82.5168235.46461.72344.892516.00468.526352.879297.6154.3396116.10371.460418.493726.00417519.7327.1388505.41141.134614.318610100-68.4937234.18-70.6343253.86432.14061.886219.68438.7119-24.6791126.49-23.6394139.95821.039713.4682-68.5533536.18-69.5181555.17730.964818.9973


### 4.2. Error Calculation Method

The error in the above table is calculated to verify the effectiveness of the proposed algorithm. Equation of them is shown as follows:Force direction error:(34)ΔF=FC−FD,where FC is the collision force direction, and FD is the calculate force direction.Relative error of force in all directions:(35)φF=FC−FDFC.Force error:(36)ΔFD=fCx2+fCy2+fCz2−fDx2+fDy2+fDz2fCx2+fCy2+fCz2.In Eq. (36), fCx,fCy,fCz and fDx,fDy,fDz are the component forces of the measured force and the calculated force, respectively.Position error:(37)ΔP=PC−PD,where PC is the known collision position, and PD is the calculated collision position.Relative error of position in all directions:(38)φP=PC−PDPC.Position error (%):(39)ΔPD=ΔPx+ΔPy+ΔPzPCx2+PCy2+PCz2.


### 4.3. Analysis of Results

Two sets of experiments are designed to reflect the effectiveness and accuracy of the algorithm proposed in this paper. Experiment 1 and experiment 2 take the magnitude of collision force and collision distance as two independent variables. Figures11 and 12 show the relative error and absolute error of each direction force and the position of the collision point in experiment 1 with the increase of the collision force. Figures 13 and 14 show the relative error and absolute error of each direction force and the position of the collision point in experiment 2 as the collision distance increases. Figure 15 shows the overall error of the force and the location of the impact point.Figure 11
Absolute error relationship in all directions in experiment 1.Figure 12
Relative error relationship in each direction in experiment 1.Figure 13
Absolute error relationship in each direction in experiment 2.Figure 14
Relative error relationship in each direction in experiment 2.Figure 15
Relative error of force and position.


#### 4.3.1. Error Analysis of Experiment 1 Results

In Figures11 and 12, the absolute error ΔFx,ΔFy,ΔFz and relative errorφFx,φFy,φFz of the force in all directions are kept within a reasonable range when the collision point is fixed, and there is no obvious change trend with the increase of the force. However, the absolute position error ΔPx,ΔPy,ΔPz and relative position error φFx,φFy,φFz in all directions have a clear tendency to decrease with increasing force. Therefore, if the method proposed in this paper is used to detect the collision point, the position accuracy of the minimum detectable collision external force calculation must be achieved.


#### 4.3.2. Error Analysis of Experiment 2 Results

Figures13 and 14 show the force error and position error of the system in different directions under the same collision external force at different collision points. If the denominator is 0 when performing the calculation in Eq. (38), the calculation result is output as 0. When using the fifth set of data in Table 3 to calculate φPy, this value is eliminated as a singular value, because the difference between the calculation results is very large. It can be seen that the absolute error ΔFx,ΔFy,ΔFz and relative error φFx,φFy,φFz of the component force in all directions fluctuate within a certain range as the distance of the collision point changes when the collision force is constant. But the absolute errorΔPx,ΔPy,ΔPz and relative error φFx,φFy,φFz in each direction of the collision position continue to increase as the distance of the collision point increases. So using this method for collision point detection needs to meet the robot’s farthest collision accuracy to meet the requirements.


#### 4.3.3. The Overall Error Analysis of the Experiment

The relative error results of the force and collision points in the two sets of experiments calculated by Eq. (36) and Eq. (39) are shown in Figure 15.It can be concluded that the error of the component force in each direction fluctuates within 5%, and the maximum relative error of the resultant force is 4.8952% from Figures12, 14, and 15. The accuracy of the dynamic force compensation algorithm proposed in this paper meets the requirements. However, it can be seen that as the distance of the collision point increases, the error keeps increasing from Figures 14 and 15. The relative error of the collision point coordinate at the position 598.61 mm farthest from the force sensor reaches the maximum value of 8.712%.


### 4.4. Error Source Analysis

It can be seen from Section4.3 that the magnitude of the force and the distance from the collision position to the sensor have an impact on the calculation accuracy. The accuracy factor ϖ is proposed to verify the impact of collision force and collision distance on the accuracy of the collision point detection algorithm proposed in this paper, which can be expressed as follows:
(40)ϖΔPm=19∑i=110ΔPmi−ΔP¯m2,m=x,y,z,(41)ϖΔPD=19∑i=110ΔPDi−ΔP¯D2.The data of experiment 1 and experiment 2 are brought into Eqs. (40) and (41), and the specific calculation results are shown in Table 4.Table 4
Accuracy factor calculation result.
Experiment 1Experiment 2ϖΔPx5.058449.1524ϖΔPy0.865640.6191ϖΔPz16.560551.3511ϖΔPD1.36728.3715The sensitivity of data to variables is reflected by the size ofϖ. In experiment 1, the experiment of collision point detection was performed using the magnitude of the force as a variable, and in experiment 2, the experiment was performed using the position of the collision force as a variable. It can be seen from Table 4 that the values of ϖΔPx, ϖΔPy, ϖΔPz, and ϖΔPD in experiment 1 are all more smaller than those in experiment 2. Therefore, the impact of the collision position on the accuracy of the algorithm is much greater than the effect of the force. The location of the collision is the first factor affecting accuracy. So it is necessary to mainly consider the algorithm accuracy of the furthest part of the robot working space when using this algorithm for single external force collision point detection.


## 4.1. Simulation Verification Experiment

A three-degree-of-freedom robot was built to perform simulation experiments to verify the correctness and effectiveness of the algorithm proposed in this paper, as shown in Figure10. The links and joints of the manipulator are center symmetrical to reduce the amount of calculation, and the center of gravity of each link is located on its own central axis. The structural parameters of each link of the three-degree-of-freedom robot used in this paper are shown in Table 1, and the material is set to alloy (ρ=2.7×103kg/m2). In Figure 10, l1=120mm, l2=150mm, l3=200mm, l4=200mm, and h=30mm.Figure 10
Robot structure.Table 1
Connecting rod parameters.
LinkQuality(kg)Center of mass coordinates in each link coordinate system (mm)01.598(0,0,60)11.686(0,0,70.024)22.489(84.685,0,0)33.201(104.684,0,0)The date of six-axis force/torque sensor at the base will change continuously over time. The data collected by the sensor at the base is preprocessed and brought into the proposed collision point detection algorithm. The robot model is imported into ADAMS, and two sets of simulation experiments are designed to verify the proposed dynamic force compensation algorithm and collision point detection algorithm. The magnitude, direction, and position of the collision force applied in these two sets of experiments are known, and the data in the experimental results are based on the sensor coordinate systemOS. The first set of experiments was designed to verify the proposed gravity compensation algorithm and collision point detection algorithm, and an equal amount of increased collision force was applied at the same collision point pc=204.32,68.96,518.46. The robot was set to be stationary in this set of experiments, and θ1=−π/6, θ2=−π/6, θ3=−π/4, and h=30mm. The results of the experiment are shown in Table 2, where the collision direction is obtained by calculating the difference of the coordinate system data of the sensor before and after the force is applied in the ADAMS. Calculation force and calculate position can be obtained by Eq. (24) and Eq. (11) and a series of coordinate transformations.Table 2
Experimental results of different external forces.
NumberCollision force (N)Collision direction (N)Collision position (mm)Calculation force (N)Calculate position (mm)Force direction error (N)Force error (%)Position error (mm)Position error (%)140-32.0642204.32-31.2463217.96820.81791.533013.65827.10220.427168.9619.997674.14600.42955.186012.4347518.4613.2315539.49480.796821.0348260-46.3017204.32-45.2675216.66471.03420.507512.34476.86433.970568.9634.289475.09080.32796.130817.3827518.4618.4023538.52271.019620.0672380-62.6532204.32-60.4687216.35562.18452.661212.03566.54149.334668.9648.675074.48500.65965.52506.3780518.466.1794539.49480.198619.16844100-81.4014204.32-78.6781192.44652.72332.848111.87356.17356.346768.9654.761473.0761.58534.116014.1018518.4615.7934537.13311.691618.67315120-95.9861204.32-98.7495194.14372.76342.187510.17635.68469.780468.9670.645164.86870.86474.091317.8147518.4617.1665500.81080.648217.64926140-114.0861204.32-117.1907214.21273.10461.54649.89275.49380.648368.9679.950965.49770.69743.46238.9562518.469.2236535.94930.267417.48937160-126.5287204.32-124.5523214.25841.97640.43029.93845.20896.167368.9697.404072.96441.23674.004418.5024518.4619.4785533.76120.976115.30128180-143.1024204.32-145.1510211.96382.04861.30907.64384.431105.867168.96106.883374.15961.01625.199626.7182518.4627.5855506.42240.867312.03769200-161.4531204.32-160.3099196.24091.14320.60928.07914.173115.614668.96114.967465.57420.64723.385823.7941518.4624.4413530.42730.647211.967310220-177.0267204.32-174.6555211.11132.37121.20276.79133.675127.346868.96126.273863.83381.09425.126229.0575518.4628.1632509.74170.89438.7183In order to verify the effectiveness and accuracy of the dynamic force compensation algorithm and collision point detection algorithm in the experiment 2, the following assumptions were added on the basis of experiment 1:θ̇1=θ̇2=θ̇3=0; θ¨1=0.3491rad/s2, θ¨2=0.1746rad/s2, and θ¨3=0.4363rad/s2. The robot moves according to the set parameters in experiment 2 and performs the collision test at different points with the same collision force when running to the 3rd second. Specific experimental data is shown in Table 3.Table 3
Experimental results of different collision points.
NumberCollision force (N)Collision direction (N)Collision position (mm)Calculation force (N)Calculate position (mm)Force direction error (N)Force error (%)Position error (mm)Position error (%)110092.687340.0394.562940.46271.87562.20440.43271.547010.348609.67450.04360.67410.043636.0828148.0337.5501149.9261.46731.8960210062.3486060.38131.34681.96732.37991.34681.785673.948338.4972.750037.64331.19830.846725.3831167.0324.3152167.89721.06790.86723100-82.943646.83-84.190547.31611.24691.9460.48612.391548.6725049.60712.37910.93462.3791-27.4107184.16-29.051485.83911.64071.6791410076.894264.0778.761566.71831.86730.75652.64832.854014.687936.8316.419538.99731.73162.1673-62.2218264.19-61.6539261.17630.56793.01375100-16.483482.64-18.050579.17091.56711.52833.46913.967984.67816.7685.66159.40810.98342.6481-50.5759306.82-51.4226313.31370.84676.49376100-45.0167116.79-46.3635122.10081.34680.87605.31084.8264-62.864369.72-62.229776.48910.63466.769163.4159298.7164.4505302.47131.03463.76137100-86.0348190.46-87.7972180.78661.76242.25009.67347.578032.186480.7333.051692.22370.865211.493739.5228422.67-40.6704408.17661.147614.4934810072.1894213.4170.7132226.90011.47620.579613.49017.779230.1609124.0829.6686135.71480.492311.6348-62.2817486.49-63.2758503.80370.994117.31379100-80.7934219.46-82.5168235.46461.72344.892516.00468.526352.879297.6154.3396116.10371.460418.493726.00417519.7327.1388505.41141.134614.318610100-68.4937234.18-70.6343253.86432.14061.886219.68438.7119-24.6791126.49-23.6394139.95821.039713.4682-68.5533536.18-69.5181555.17730.964818.9973


## 4.2. Error Calculation Method

The error in the above table is calculated to verify the effectiveness of the proposed algorithm. Equation of them is shown as follows:Force direction error:(34)ΔF=FC−FD,where FC is the collision force direction, and FD is the calculate force direction.Relative error of force in all directions:(35)φF=FC−FDFC.Force error:(36)ΔFD=fCx2+fCy2+fCz2−fDx2+fDy2+fDz2fCx2+fCy2+fCz2.In Eq. (36), fCx,fCy,fCz and fDx,fDy,fDz are the component forces of the measured force and the calculated force, respectively.Position error:(37)ΔP=PC−PD,where PC is the known collision position, and PD is the calculated collision position.Relative error of position in all directions:(38)φP=PC−PDPC.Position error (%):(39)ΔPD=ΔPx+ΔPy+ΔPzPCx2+PCy2+PCz2.


## 4.3. Analysis of Results

Two sets of experiments are designed to reflect the effectiveness and accuracy of the algorithm proposed in this paper. Experiment 1 and experiment 2 take the magnitude of collision force and collision distance as two independent variables. Figures11 and 12 show the relative error and absolute error of each direction force and the position of the collision point in experiment 1 with the increase of the collision force. Figures 13 and 14 show the relative error and absolute error of each direction force and the position of the collision point in experiment 2 as the collision distance increases. Figure 15 shows the overall error of the force and the location of the impact point.Figure 11
Absolute error relationship in all directions in experiment 1.Figure 12
Relative error relationship in each direction in experiment 1.Figure 13
Absolute error relationship in each direction in experiment 2.Figure 14
Relative error relationship in each direction in experiment 2.Figure 15
Relative error of force and position.


### 4.3.1. Error Analysis of Experiment 1 Results

In Figures11 and 12, the absolute error ΔFx,ΔFy,ΔFz and relative errorφFx,φFy,φFz of the force in all directions are kept within a reasonable range when the collision point is fixed, and there is no obvious change trend with the increase of the force. However, the absolute position error ΔPx,ΔPy,ΔPz and relative position error φFx,φFy,φFz in all directions have a clear tendency to decrease with increasing force. Therefore, if the method proposed in this paper is used to detect the collision point, the position accuracy of the minimum detectable collision external force calculation must be achieved.


### 4.3.2. Error Analysis of Experiment 2 Results

Figures13 and 14 show the force error and position error of the system in different directions under the same collision external force at different collision points. If the denominator is 0 when performing the calculation in Eq. (38), the calculation result is output as 0. When using the fifth set of data in Table 3 to calculate φPy, this value is eliminated as a singular value, because the difference between the calculation results is very large. It can be seen that the absolute error ΔFx,ΔFy,ΔFz and relative error φFx,φFy,φFz of the component force in all directions fluctuate within a certain range as the distance of the collision point changes when the collision force is constant. But the absolute errorΔPx,ΔPy,ΔPz and relative error φFx,φFy,φFz in each direction of the collision position continue to increase as the distance of the collision point increases. So using this method for collision point detection needs to meet the robot’s farthest collision accuracy to meet the requirements.


### 4.3.3. The Overall Error Analysis of the Experiment

The relative error results of the force and collision points in the two sets of experiments calculated by Eq. (36) and Eq. (39) are shown in Figure 15.It can be concluded that the error of the component force in each direction fluctuates within 5%, and the maximum relative error of the resultant force is 4.8952% from Figures12, 14, and 15. The accuracy of the dynamic force compensation algorithm proposed in this paper meets the requirements. However, it can be seen that as the distance of the collision point increases, the error keeps increasing from Figures 14 and 15. The relative error of the collision point coordinate at the position 598.61 mm farthest from the force sensor reaches the maximum value of 8.712%.


## 4.3.1. Error Analysis of Experiment 1 Results

In Figures11 and 12, the absolute error ΔFx,ΔFy,ΔFz and relative errorφFx,φFy,φFz of the force in all directions are kept within a reasonable range when the collision point is fixed, and there is no obvious change trend with the increase of the force. However, the absolute position error ΔPx,ΔPy,ΔPz and relative position error φFx,φFy,φFz in all directions have a clear tendency to decrease with increasing force. Therefore, if the method proposed in this paper is used to detect the collision point, the position accuracy of the minimum detectable collision external force calculation must be achieved.


## 4.3.2. Error Analysis of Experiment 2 Results

Figures13 and 14 show the force error and position error of the system in different directions under the same collision external force at different collision points. If the denominator is 0 when performing the calculation in Eq. (38), the calculation result is output as 0. When using the fifth set of data in Table 3 to calculate φPy, this value is eliminated as a singular value, because the difference between the calculation results is very large. It can be seen that the absolute error ΔFx,ΔFy,ΔFz and relative error φFx,φFy,φFz of the component force in all directions fluctuate within a certain range as the distance of the collision point changes when the collision force is constant. But the absolute errorΔPx,ΔPy,ΔPz and relative error φFx,φFy,φFz in each direction of the collision position continue to increase as the distance of the collision point increases. So using this method for collision point detection needs to meet the robot’s farthest collision accuracy to meet the requirements.


## 4.3.3. The Overall Error Analysis of the Experiment

The relative error results of the force and collision points in the two sets of experiments calculated by Eq. (36) and Eq. (39) are shown in Figure 15.It can be concluded that the error of the component force in each direction fluctuates within 5%, and the maximum relative error of the resultant force is 4.8952% from Figures12, 14, and 15. The accuracy of the dynamic force compensation algorithm proposed in this paper meets the requirements. However, it can be seen that as the distance of the collision point increases, the error keeps increasing from Figures 14 and 15. The relative error of the collision point coordinate at the position 598.61 mm farthest from the force sensor reaches the maximum value of 8.712%.


## 4.4. Error Source Analysis

It can be seen from Section4.3 that the magnitude of the force and the distance from the collision position to the sensor have an impact on the calculation accuracy. The accuracy factor ϖ is proposed to verify the impact of collision force and collision distance on the accuracy of the collision point detection algorithm proposed in this paper, which can be expressed as follows:
(40)ϖΔPm=19∑i=110ΔPmi−ΔP¯m2,m=x,y,z,(41)ϖΔPD=19∑i=110ΔPDi−ΔP¯D2.The data of experiment 1 and experiment 2 are brought into Eqs. (40) and (41), and the specific calculation results are shown in Table 4.Table 4
Accuracy factor calculation result.
Experiment 1Experiment 2ϖΔPx5.058449.1524ϖΔPy0.865640.6191ϖΔPz16.560551.3511ϖΔPD1.36728.3715The sensitivity of data to variables is reflected by the size ofϖ. In experiment 1, the experiment of collision point detection was performed using the magnitude of the force as a variable, and in experiment 2, the experiment was performed using the position of the collision force as a variable. It can be seen from Table 4 that the values of ϖΔPx, ϖΔPy, ϖΔPz, and ϖΔPD in experiment 1 are all more smaller than those in experiment 2. Therefore, the impact of the collision position on the accuracy of the algorithm is much greater than the effect of the force. The location of the collision is the first factor affecting accuracy. So it is necessary to mainly consider the algorithm accuracy of the furthest part of the robot working space when using this algorithm for single external force collision point detection.


## 5. Conclusion and Future Work

The detection of collision by the robot can make the robot interact with the environment, so that the robot can adapt to the environment and realize the safety of human-computer interaction. Collision sensing includes collision position, collision direction, and force size. Most of the existing methods use geometric constraints. Their disadvantages are that they cannot be applied to unstructured environments and cannot cope with the deformation of collision objects. In order to make up for the above problems, a robot collision point detection algorithm for single point and single external force is proposed in this paper, and the gravity compensation and dynamic force compensation algorithms of the six-axis force/torque sensors at the base are described, then perform simulation experiments to verify the algorithm. Compared with the traditional algorithms [18, 19] that use the geometric information of the collision surface to calculate the collision position, the algorithm proposed in this paper has a minimum error of only 1.5470%, and the maximum error is approximated by traditional methods. However, the advantage compared with the traditional method is that the proposed algorithm does not rely on the geometric information of the collision body surface, and the generation of deformation has no effect on the result. The experimental results prove the effectiveness of the proposed algorithm and show that the accuracy of the proposed gravity and dynamic force compensation algorithm does not change with the change of the force and the position of the collision point. The accuracy of the collision point detection algorithm is obviously affected by the magnitude of the collision force and the collision position. The accuracy of the algorithm in the furthest part of the robot’s workspace should be considered first when applying this algorithm.The application of the algorithm proposed in this paper in robot force drag teaching and application in medical collaborative robots needs to continue to be explored, which could be as follows: (1) how to use contact information as a basis for decision-making to achieve task division in human-machine collaboration and (2) how to use the trajectory of the touch point as the basis to realize the force teaching of the complex curved surface of the robot. Aiming at the application direction of the above two points, experimental research will be carried out in the follow-up to increase the practicality of the algorithm proposed in this paper.



---

*Source: 8881263-2020-12-12.xml*