Servo Collision Detection Control System Based on Robot Dynamics

Abstract

Collision detection and inspection of industrial robots have become essential functions in modern industrial automation. Sensor-based detection methods are commonly employed in research to achieve collision detection, including high-precision force sensors, ultrasonic ranging sensors, electronic skins, and others. While collision detection using force sensors or electronic skin sensors offers very high accuracy, the inclusion of these sensors increases the overall cost. This article proposes a solution using dynamic modeling for collision detection. First, the theoretical torque generated by each axis of the industrial robot under different pose conditions is analyzed in real time. Then, the actual torque is calculated by sampling the motor current of each axis. By setting error margins and collision detection thresholds, collision detection can be achieved in a cost-effective manner without the need for additional sensors. Experiments were conducted to evaluate this dynamic modeling approach to collision detection. The findings indicated that the approach is efficacious and capable of identifying the impacts of diverse collision objects. However, compared to sensor-based detection methods, collision detection using dynamic modeling has the disadvantage of lower accuracy. Future research will concentrate on enhancing the calculation accuracy of the theoretical torque to enhance the sensitivity of collision detection.

1. Introduction

In recent years, as factory automation has advanced, the use of industrial robots has steadily increased. These robots have become an integral part of automated production lines, playing key roles in applications such as welding, material handling, sampling, assembly, and more [1]. As the functional requirements for industrial robots grow, collision detection has become an essential feature. Robots equipped with collision detection capabilities can protect both the robot body and control system, preventing significant damage in the occurrence of a collision [2,3].

Common collision detection methods for industrial robots include torque sensors, dual encoders, ultrasonic sensors, and other sensor-based approaches. Reference [4] introduces a novel distance evaluation methodology based on depth sensors for collision detection, representing a significant advancement in robotic collision detection techniques. References [5,6] employed a data-driven approach utilizing coupled dynamic data from manipulators with and without external contacts to design and train a multi-output neural network. This innovative framework enables the detection of unintended collisions and identification of impacted links solely through the manipulator’s intrinsic joint position and torque sensors, eliminating the need for additional external sensing devices. Reference [7] proposes a systematic methodology for quantifying the impact of current sensor drift on human–robot collision behavior, accompanied by the development of a data-driven compensation approach. In Reference [8], a novel methodology was introduced for collision detection by monitoring current variations in each joint of the robotic system. This approach leverages force sensor data mounted at the robot’s base, employing the principles of virtual displacement and force method to accurately determine the collision point location information at the impacted joint.

This article focuses on a collision detection method based on robot dynamics, which requires the development of an accurate robot dynamics model. By monitoring the motor currents of each axis, the torque output to each joint is calculated using motor parameters and the joint transmission ratio. Additionally, the position encoder provides feedback, which, when combined with the robot dynamics equations, helps determine the required driving torque for each joint in the current motion state. The calculated torque values are then compared to detect potential collisions. The dynamics of robots can be modeled using methods such as the Euler–Lagrange approach or the Newton–Euler equation, which analyze the relationship between joint motion and joint torque [9]. In addition, because robots use different loads in their actual work, it is necessary to set tool load parameters for the robot and incorporate them into dynamic calculations. In the process of modeling, it is necessary to accurately know the mechanical and motor parameters of industrial robots and calculate the speed and acceleration based on the feedback positions of each axis motor, as well as the theoretical torque, in order to ultimately determine the occurrence of collisions [10]. Since the theoretical torque is computed instantaneously according to the robot’s pose and the feedback torque is also sampled in real time, collisions can be detected promptly in various operating states [11].

In comparison with other methods, Reference [12] directly uses changes in motor current as an indicator of robot collisions. When a robot collides, its joints will suddenly be subjected to the torque produced by the collision force, which will change the load on the corresponding joints and cause a sudden change in the motor current. Therefore, the signal obtained by differentiating the current over time can be used as the collision detection signal. When the robot changes direction, it can also cause sudden changes in the robot’s joint torque, so the joint speed needs to be added to the detection signal. Through the experimental results of Reference [12], it was found that the method proposed in Reference [12] has the advantage of high accuracy. The detection threshold for joint 1 is only 0.5 NM, but the real-time performance is not high, with collision detection taking about 5–6 ms. Due to its ability to detect collisions through feedback torque, there is a possibility of misjudgment. Besides collisions, there are other situations that can cause an abnormal torque increase, such as motor brake damage, motor damage, or mechanical problems. This leads to misjudgment in this method. According to Reference [13], by utilizing CAD and setting up an impedance controller, collisions between robots can be effectively prevented. From the experimental results in Reference [13], the collision avoidance effect is relatively good, but the real-time performance is not high. The communication cycle between the controller and the PC reaches 150 ms, and it relies on CAD. Reference [14] demonstrates that using torque sensors provides a relatively simple, convenient, and effective method for detecting whether a robot has collided. The feedback torque based on torque sensors offers higher accuracy, but it still relies on the accuracy of the model. According to the experimental results in Reference [14], the torque sensor used exhibits high accuracy, with a threshold set at 0.8 Nm, and shows good performance in terms of real-time capability and false positive rate. However, the use of force sensors increases both structural and component costs.

Table 1. Collision detection principles and handling methods of some manufacturers.

Robot	Components Used for Collision Detection	Detection Mechanism	Processing Results
UR5(UR)	Dual encoder	Torque/current	Emergency stop
YuMi(ABB)	None	Current	Reverse turn
LBR iiwa(KUKA)	Dual encoder and Torque sensor	Torque	Reverse turn
CR-35iA(FANUC)	Torque sensor	Torque	Emergency stop

shows the methods currently used by some robot manufacturers to detect collision faults, which are mostly based on dual encoders and torque sensors.

2. Servo (Chengdu CRP Robot Technology Co., Ltd., Chengdu, China) System with Collision Detection Function

2.1. Hardware Platform

Figure 1 shows the complete servo hardware topology, with the servo driver as the core component for the robot’s collision detection system. Let us first discuss its hardware framework.

As the core component of the driving motor, servo drives play a crucial role in receiving commands from the main controller and controlling the motor in industrial robots. The entire hardware circuit includes a rectifier circuit, an inverter circuit, a digital signal processor (DSP) control circuit, and a sampling circuit. The rectifier circuit transforms three-phase alternating current into direct current. The power circuit’s primary function is to transform 220 VAC into various DC voltages, including VCC (5 V), 3.3 VDC, and ±15 V. These voltages primarily power the DSP and various other components like sampling chips, driver chips, and operational amplifier chips, as depicted in Figure 1. The inverter circuit includes a pulse-width modulation (PWM) drive circuit and an insulated gate bipolar transistor (IGBT). The PWM circuit is a key pulse-generating device for motor control. It produces a high-speed switching PWM signal that directly drives the motor, enabling it to rotate. This circuit is a fundamental part of the power system. The PWM driver chip model is 1ED3121, shown in orange in Figure 1. 1ED3121 is an IGBT gate driver chip designed for easy integration into reliable systems. The IGBT module, shown in blue in Figure 1, is responsible for switching the power to the motor.

The DSP control circuit includes the DSP core, along with analog and digital signal acquisition and processing circuits. As the core of servo control, DSP is the central processing unit of the entire servo system. Analog signals include motor current, bus voltage, and temperature, all of which are sampled by the DSP. After signal conditioning, they enter the DSP ADC channel for control and calculation. Digital signal processing circuits mainly include PWM wave generation, state quantity control, digital input signals, overcurrent protection signals, etc. [15]. Additionally, the DSP is responsible for communication with the main controller, including data processing through the Controller Area Network (CAN) for communication with the host computer. It also stores data and supports other functions. The DSP is connected to the main controller through communication methods such as Ethernet for Control Automation Technology (ECAT) communication, receives position and speed commands from the main controller, and feeds back information such as motor current, motor speed, and motor position sampled by the servo to the main controller. It form a closed-loop control loop [16].

2.2. Software Framework

As seen in Figure 2, the software control framework makes use of TI’s TMS320F28377D processor, a potent 32-bit floating-point MCU made for sophisticated closed-loop control applications, including servo motor control and industrial drives. This dual-core DSP operates at a maximum clock frequency of 200 MHz and provides 24 PWM peripheral interfaces, making it an ideal choice for controlling multiple motors. The chip can control up to four motors and offers a wide range of peripheral interfaces. In this paper, the PWM frequency is set to 10 kHz, with a control period and sampling period both of 100 µs. The clock frequency is 2.5 MHz, giving a clock cycle of 0.4 µs. It takes approximately 12.8 µs to sample data from one encoder and 25.6 µs to sample data from two encoders. The remaining time for other processes is 74.4 µs, which is sufficient. However, reducing the sampling time and increasing the sampling frequency could still offer benefits [17].

The entire software process begins with the communication module, which handles the sending and receiving of data. This module facilitates data exchange between the main controller and the servo system. It receives enable commands, position commands, speed commands, current commands, and stop commands from the main controller while also uploading information about the servo’s status, position, feedback speed, and motor current. Communication methods can vary, but stability in the communication cycle is critical. A typical communication cycle is 1–2 ms. The commonly used communication protocol is the ECAT communication protocol, which is an industrial Ethernet control protocol developed by the German company Beckhoff. It is very suitable for motion control. This article uses 28377D and requires the use of specialized ECAT slave data processing chips. Currently, specialized ECAT chips include ET1100 and LAN9252. If TI’s 28,388 is used, there is no need for a dedicated slave processing chip, as this chip already integrates the ECAT protocol’s processing functionality. In addition, it is also possible to use dual-port RAM instead of bus communication. The primary controller and servo may concurrently read from and write to the dual-port RAM, facilitating interaction between the main controller and servo data [18].

When the servo is ready, it waits for the main control command. Upon receiving the enable command from the main controller, the servo enters the enabled state and can receive position and speed commands. The main controller can also send only the position command without the speed command, if necessary [19]. While the main controller controls the servo’s position, the speed command, when provided, serves as a feedforward input to the speed control loop, potentially improving control performance. At the same time, the DSP continuously collects real-time data such as motor current, bus voltage, and temperature. The DSP then performs closed-loop control to adjust the motor and bring it to the desired position as specified by the main controller [20]. Additionally, the theoretical torque for each axis is computed according to the position, velocity, and acceleration data. The feedback torque is calculated from the motor current, and both values are compared to the collision threshold. If the threshold is exceeded, a collision warning is triggered, and the servo is activated to issue a collision alert. Otherwise, the motor control process continues as normal. Throughout the entire process, the servo will detect its own status in real time to see if any faults have occurred. If other faults occur, the servo will also be activated and enter a fault state, facilitating timely protection of the servo and robot [21].

2.3. Control Framework

The entire control framework, as shown in Figure 3, consists of three servo protection control loops: the position control loop, the speed control loop, and the current control loop. The current control loop functions as the innermost loop, followed by the speed control loop as the middle loop and the position control loop as the outermost loop. After sampling the actual current of the motor with DSP, mathematical transformation is necessary for control. The motor current, which is a three-phase alternating current, is disadvantageous for control purposes [22].

In Figure 3, ${\theta }^{*}$ represents the position command transmitted from the main controller to the servo system, while ${\omega }^{*}$ denotes the output of the position loop, which simultaneously serves as the speed reference for the speed loop. The quantity ${I^{*}}_q$ corresponds to the output of the speed loop and functions as the current reference for the q-axis current loop, designated as A_Iq_R. Similarly, ${I^{*}}_d$ represents the current reference for the d-axis current loop, labeled as A_Id_R. For permanent magnet synchronous motors, ${I^{*}}_d$ is typically maintained at zero. The variable ${u^{*}}_q$ signifies the output of the q-axis current loop A_Iq_R and concurrently acts as one of the inputs to the Inverse Park Transformation module. Likewise, ${u^{*}}_d$ represents the output of the d-axis current loop A_Id_R and serves as another input to the Inverse Park Transformation module. Following this transformation, ${u^{*}}_{\alpha }$ and ${u^{*}}_{\beta }$ are generated as outputs of the Inverse Park Transformation module and subsequently serve as inputs to the Space Vector Pulse Width Modulation (SVPWM) Algorithm module.

To facilitate control, the three-phase alternating current must first be converted into two-phase orthogonal alternating current using mathematical formulas. This is followed by converting the two-phase alternating current into a two-phase direct current. This process involves the Clark transformation and the Park transformation. After applying these two transformations, the motor current is expressed as direct currents $I_d$ and $I_q$. They are the feedback of the d-axis current loop and the q-axis current loop, respectively [23]. The d-axis current is usually set to zero when controlling permanent magnet synchronous motors. The output of the q-axis current loop is determined by the speed loop, thereby forming two current control loops. In practical engineering applications, it is also necessary to filter the feedback and give the current separately. Here, $I_q$ also needs to calculate the feedback torque. As $I_q$ is the torque current of the motor, it must be transformed into each axis’s feedback torque. The conversion formula is as follows:

$$T_r=I_q\times Q_r\times G\times R$$

(1)

In the Formula (1), $T_r$ is the feedback torque, $I_q$ is the torque current of the motor, $Q_r$ is the rated torque of the motor, G is the reduction ratio, and R is the direction of rotation, with values of 1 or −1. In addition to the current control loops, there is also the speed loop, which regulates the speed of the motor. The feedback speed is acquired using differentiating the encoder position. The speed setpoint for the speed loop is the position loop’s output. In engineering applications, filtering is also required for both the feedback speed and the given speed. The outermost loop is the position loop, which regulates the position of the motor. The position feedback is obtained from the encoder, while the position setpoint is provided by the main controller. Since the control cycle of the motor is usually shorter than the communication cycle, interpolation filtering is often used during the position-setting process. This completes the motor control framework as discussed in this paper [24].

How can we exactly acquire the torque current, $I_q$? To obtain the torque current, $I_q$, for closed-loop current control, a series of transformations must be performed on the sampled motor current. The process of calculating $I_q$ is shown in Figure 4.

DSP calculates the torque current, $I_q$, by collecting the actual motor current. The conversion from a three-phase alternating current (AC) to a two-phase alternating current is called Clark transformation, as shown in Formulas (2) and (3).

$$I_{\alpha }={\displaystyle \frac{3}{2}}{I}_u$$

(2)

$$I_{\beta }={\displaystyle \frac{\sqrt{3}}{2}}{I}_v-{\displaystyle \frac{\sqrt{3}}{2}}{I}_w$$

(3)

In the above equation, $I_u$ $I_v$ and $I_w$ represent the actual currents of the motor. $I_{\alpha }$ and $I_{\beta }$ are the currents after Clark transformation [25,26]. After applying the Clark transformation, the three-phase AC, exhibiting a phase difference of 120 degrees during motor rotation, is transformed into a two-phase AC with a phase difference of 90 degrees during motor rotation.

These two-phase orthogonal AC signals $I_{\alpha }$ and $I_{\beta }$ will be converted into DC currents $I_q$ and $I_d$ after undergoing Park transform.

The Park transformation formula is as follows:

$$I_d=I_{\alpha }\mathit{cos}{\theta }_d+I_{\beta }\mathit{sin}{\theta }_d$$

(4)

$$I_q={-I}_{\alpha }\mathit{sin}{\theta }_d+I_{\beta }\mathit{cos}{\theta }_d$$

(5)

A is the electrical angle, and its calculation formula is as follows:

$${\theta }_d=(\theta +\mathsf{\Delta }\theta )\times P$$

(6)

Θ is the angle of the encoder, and $\mathsf{\Delta }\theta$ is the electrical angle offset, which could be 0°, 90°, 180°, or other angles. Typically, these data are provided by the motor manufacturer. If there are no electrical angle offset data, the $I_d$ current loop can be used to control and determine the electrical angle offset, $\mathsf{\Delta }\theta$. However, since this is not the focus of this paper, the specific methods will not be described in detail. P is the number of motor poles.

By using Formulas (2)–(6) and following the steps shown in Figure 4, the torque current, $I_q$, can be obtained.

3. Collision Detection Dynamic Modeling

3.1. Kinematic Modeling

Kinematics and dynamics analysis are fundamental for the study of industrial robots. Kinematic analysis refers to the static analysis of robots, which delineates the correlation between a robot’s joint angles and the posture of its end effector. Kinematics is divided into forward kinematics and inverse kinematics. The inverse solution determines the joint angles in accordance with the desired end effector pose, whereas the forward solution determines the robot’s end effector position based on its joint angles. This is the foundation of robot trajectory planning. This article focuses on a robot with a 50 kg load. Based on its structure, its coordinates are initially established using the six-axis coordinate system (6) from the base coordinate system (0). Then, modified D-H parameters will be established based on the coordinates. The D-H parameter method is a coordinate transformation approach based on homogeneous transformation matrices, originally proposed in 1955 by Denavit and Hartenberg. Using a 4 $\times$ 4 homogeneous transformation matrix, this technique explains the spatial link between adjacent robot linkages and the robot’s kinematics through homogeneous transformations. The D-H approach is the name of this modeling technique. The D-H approach is still frequently applied today and is crucial to the advancement of robotics [27,28].

The regular D-H approach and the modified D-H method are now the two main categories of D-H modeling techniques. The primary distinction between these two approaches is that the connecting rod coordinate system {i} is positioned differently. The coordinate system {i} is established on the axis of the i + 1 joint by the normal D-H approach, whereas the coordinate system {i} is established on the axis of the i joint by the modified D-H method. Obviously, this alteration also results in variations in the D-H matrix and D-H parameters. The standard D-H method, proposed by Denavit and Hartenberg, is primarily designed for serial mechanisms. However, it can face challenges when modeling tree structures (such as when two branches are connected at the end of a linkage) or closed-chain structures. In contrast, the improved D-H approach avoids these problems; it is more adaptable and suitable for a wider range of robotic systems [29].

Determining the modified D-H parameters is essential to building the kinematic model of industrial robots. To establish these parameters, it is essential to first define a coordinate system for the six axes of the industrial robot. The six-axis coordinate system, along with parameters established for a 50 kg load robot, is displayed in Figure 5b. Figure 5a illustrates the actual industrial robot used in the project [30].

Once the modified D-H coordinate system is defined, the modified D-H parameter table can be filled in based on the actual robot’s link length and link offset data. In the D-H parameter table, $a_{i-1}$ is the length of connecting link i − 1, defined as the length of the common normal between the axis of joint i − 1. Essentially, it is the shortest spatial distance between the axes of two consecutive joints. ${\alpha }_{i-1}$ is the connecting link i − 1 torsion angle, which is the angle pointing from axis i − 1 to axis i between the axis of joint i − 1 and the axis of joint i. $d_i$ is the offset of link i relative to link i − 1, defined as the distance between the two common normal lines, $a_i$ and $a_{i-1}$, on joint i. ${\theta }_i$ is the joint angle, which is the angle at which connecting link i rotates around axis i in relation to connecting rod i − 1. It is measured around joint axis i and points from $a_{i-1}$ to $a_i$.

After the updated D-H coordinate system has been established and the changed D-H parameters for the neighboring links have been established, the relationship between the end effector pose, joint angles (θ), and modified D-H parameters can be obtained by the use of the modified D-H transformation matrix. From the knowledge of robotics, the coordinate system {i} transformation matrix formula with respect to coordinate system {i − 1} is as follows:

$$\begin{gathered} {}_i{}^{i-1}T=Rot\left(X_{, }{\alpha }_{i-1}\right)Trans\left(X_{, }{a}_{i-1}\right)Rot(Z,{\theta }_i)Trans(Z, d_i) \\ =\left[\begin{array}{c}{\mathit{cos}\theta }_i\\ \begin{array}{c}{\mathit{sin}\theta }_i{\mathit{cos}\alpha }_{i-1}\\ \begin{array}{c}{\mathit{sin}\theta }_i{\mathit{sin}\alpha }_{i-1}\\ 0\end{array}\end{array}\end{array} \begin{array}{c}{-sin\theta }_i\\ \begin{array}{c}{\mathit{cos}\theta }_i{\mathit{cos}\alpha }_{i-1}\\ \begin{array}{c}{cos\theta }_i{\mathit{sin}\alpha }_{i-1}\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}-sin{\alpha }_{i-1}\\ \begin{array}{c}{cos\alpha }_{i-1}\\ 0\end{array}\end{array}\end{array} \begin{array}{c}{ a}_{i-1}\\ \begin{array}{c}-sin{\alpha }_{i-1}{d}_i\\ \begin{array}{c}{cos\alpha }_{i-1}{d}_i\\ 1\end{array}\end{array}\end{array}\right] \end{gathered}$$

(7)

In Formula (7), ${}_i{}^{i-1}T$ represents the pose transformation matrix from joint i to joint i − 1, $Rot(X,{ \alpha }_{i-1})$ represents a rotation of ${\alpha }_{i-1}$ degrees around the X-axis, $Trans(X,{ \alpha }_{i-1})$ represents a translation of ${ a}_{i-1}$ meters along the X-axis, $Rot(Z,{\theta }_i)$ represents a rotation of ${ \theta }_{i-1}$ degrees around the Z-axis, and $Trans(Z, d_i)$ represents a translation of $d_i$ meters along the Z-axis [31].

For an industrial robot with six joints, when the updated D-H coordinate system has been established and the adjusted D-H parameters between neighboring coordinate systems have been established, six modified D-H matrices can be calculated. By sequentially multiplying these matrices, the robot’s kinematic model is obtained. The kinematic model of a six-joint industrial robot is as follows:

$$\begin{gathered} {}_6{}^{0}T={}_1{}^{0}T{}_2{}^{1}T{}_3{}^{2}T{}_4{}^{3}T{}_5{}^{4}T{}_6{}^{5}T \\ ={}_1{}^{0}T\left({\theta }_1\right){}_2{}^{1}T\left({\theta }_2\right){}_3{}^{2}T\left({\theta }_3\right){}_4{}^{3}T\left({\theta }_4\right){}_5{}^{4}T\left({\theta }_5\right){}_6{}^{5}T{(\theta }_1) \end{gathered}$$

(8)

${}_6{}^{0}T$ represents the pose transformation matrix from joint 6 to the base coordinate system, ${}_1{}^{0}T$ represents the pose transformation matrix from joint 1 to base coordinate system (0), and so on. According to Formula (8) and the improved D-H parameter in

Table 2. Modified D-H parameter table.

Coordinate System Transformation	Link	${\mathit{\alpha }}_{\mathit{i}-1}(°)$	${\mathit{a}}_{\mathit{i}-1}$/mm	${\mathit{\theta }}_{\mathit{i}}(°)$	${\mathit{d}}_{\mathit{i}}$/m
Base–Link 1	1	0	0	${\theta }_1\left(0\right)$	0
Link 1–Link 2	2	90	320	${\theta }_2\left(90\right)$	0
Link 2–Link 3	3	0	1111	${\theta }_3\left(0\right)$	0
Link 3–Link 4	4	90	205	${\theta }_4\left(0\right)$	0
Link 4–Link 5	5	−90	0	${\theta }_5\left(0\right)$	1232
Link 5–Link 6	6	90	0	${\theta }_6\left(0\right)$	169

, it can be calculated that ${}_1{}^{0}T\left({\theta }_1\right)$, ${}_2{}^{1}T\left({\theta }_2\right)$, ${}_3{}^{2}T\left({\theta }_3\right)$, ${}_4{}^{3}T\left({\theta }_4\right)$, ${}_5{}^{4}T\left({\theta }_5\right)$, ${}_6{}^{5}T\left({\theta }_6\right)$. Then, how the robot’s final position is represented in the base coordinate system 0 is determined.

$${}_1{}^{0}T\left({\theta }_1\right)=\left[\begin{array}{c}{\mathit{cos}\theta }_1\\ \begin{array}{c}{\mathit{sin}\theta }_1\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}-{\mathit{sin}\theta }_1\\ \begin{array}{c}{\mathit{cos}\theta }_1\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}0\\ \begin{array}{c}1\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}0\\ \begin{array}{c}0\\ 1\end{array}\end{array}\end{array}\right]$$

(9)

$${}_2{}^{1}T\left({\theta }_2\right)=\left[\begin{array}{c}{\mathit{cos}\theta }_2\\ \begin{array}{c}{\mathit{sin}\theta }_2\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}-{\mathit{sin}\theta }_2\\ \begin{array}{c}{\mathit{cos}\theta }_2\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}0\\ \begin{array}{c}1\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}0\\ \begin{array}{c}0\\ 1\end{array}\end{array}\end{array}\right]$$

(10)

$${}_3{}^{2}T\left({\theta }_3\right)=\left[\begin{array}{c}{\mathit{cos}\theta }_3\\ \begin{array}{c}{\mathit{sin}\theta }_3\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}-{\mathit{sin}\theta }_3\\ \begin{array}{c}0\\ \begin{array}{c}{\mathit{cos}\theta }_3\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}-1\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0.32\\ \begin{array}{c}0\\ \begin{array}{c}0\\ 1\end{array}\end{array}\end{array}\right]$$

(11)

$${}_4{}^{3}T\left({\theta }_4\right)=\left[\begin{array}{c}{\mathit{cos}\theta }_4\\ \begin{array}{c}{\mathit{sin}\theta }_4\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}-{\mathit{sin}\theta }_4\\ \begin{array}{c}{\mathit{cos}\theta }_4\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}0\\ \begin{array}{c}1\\ 0\end{array}\end{array}\end{array} \begin{array}{c}1.111\\ \begin{array}{c}0\\ \begin{array}{c}0\\ 1\end{array}\end{array}\end{array}\right]$$

(12)

$${}_5{}^{4}T\left({\theta }_5\right)=\left[\begin{array}{c}{\mathit{cos}\theta }_5\\ \begin{array}{c}0\\ \begin{array}{c}{\mathit{sin}\theta }_5\\ 0\end{array}\end{array}\end{array} \begin{array}{c}-{\mathit{sin}\theta }_5\\ \begin{array}{c}0\\ \begin{array}{c}{\mathit{cos}\theta }_5\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}-1\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0.205\\ \begin{array}{c}-1.232\\ \begin{array}{c}0\\ 1\end{array}\end{array}\end{array}\right]$$

(13)

$${}_2{}^{1}T\left({\theta }_2\right)=\left[\begin{array}{c}{\mathit{cos}\theta }_2\\ \begin{array}{c}{\mathit{sin}\theta }_2\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}-{\mathit{sin}\theta }_2\\ \begin{array}{c}{\mathit{cos}\theta }_2\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}0\\ \begin{array}{c}1\\ 0\end{array}\end{array}\end{array} \begin{array}{c}0\\ \begin{array}{c}0\\ \begin{array}{c}0\\ 1\end{array}\end{array}\end{array}\right]$$

(14)

Thus, the equation between ${}_6{}^{0}T$ and D-H parameters can be calculated.

$$\begin{gathered} {}_6{}^{0}T={}_1{}^{0}T\left({\theta }_1\right){}_2{}^{1}T\left({\theta }_2\right){}_3{}^{2}T\left({\theta }_3\right){}_4{}^{3}T\left({\theta }_4\right){}_5{}^{4}T\left({\theta }_5\right){}_6{}^{5}T\left({\theta }_1\right) \\ =\left[\begin{array}{c}{n}_x\\ \begin{array}{c}{n}_y\\ \begin{array}{c}{n}_z\\ 0\end{array}\end{array}\end{array} \begin{array}{c}{o}_x\\ \begin{array}{c}{o}_y\\ \begin{array}{c}{o}_z\\ 0\end{array}\end{array}\end{array} \begin{array}{c}{a}_x\\ \begin{array}{c}{a}_y\\ \begin{array}{c}{a}_z\\ 0\end{array}\end{array}\end{array} \begin{array}{c}{p}_x\\ \begin{array}{c}{p}_y\\ \begin{array}{c}{p}_z\\ 1\end{array}\end{array}\end{array}\right]=\left[ \begin{array}{cc}{}_6{}^{0}R\left(\theta \right)& {}_6{}^{0}p\left(\theta \right)\\ 0& 1\end{array} \right] \end{gathered}$$

(15)

In Equation (15), $\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]$ is the pose coordinate of the end effector of the robot, represented by ${}_6{}^{0}R\left(\theta \right)$. $⌊\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}⌋$ is the position coordinate of the robot’s end effector, represented by ${ p}_6^0\left(\theta \right)$.

3.2. Dynamics Modeling

The dynamic collision detection function studied in this article is integrated into the servo system. Since the main task of the servo is to drive the motor, the collision detection function is considered a functional module of the servo. This servo-based collision detection is rooted in robot dynamics, meaning that dynamic modeling must be performed first [32].

Dynamic analysis explains the connection between industrial robots’ joint motion and joint torque. The required torque can be computed according to the current motion state of the robot’s joints, which is crucial for dynamic collision detection. For industrial robot control, kinematic and dynamic analysis facilitates the control of robot position and force. For collision detection of industrial robots, kinematic and dynamic analysis can easily obtain information such as the position, velocity, and torque of each joint of the robot in a certain state, which facilitates the judgment of the working state of the industrial robot and combined with actual torque feedback, determines the occurrence of collision events. Therefore, dynamic analysis forms the foundation for effective dynamic collision detection.

Methods for dynamic analysis of industrial robots include the Newton–Euler method, Kane method, and Euler–Lagrange method [33,34]. The forward and inverse solutions of robot kinematics reflect the relationship between robot joint angles and robot end effector positions, while the Jacobi matrix reflects the relationship between robot joint rotation speed and end effector linear and angular velocities. Therefore, the Jacobian matrix is a critical component in both the analysis and control of robot motion, playing a key role in deriving dynamic equations and collision detection algorithms.

3.2.1. Jacobi Matrix

The inverse kinematics solution at the position level can be divided into geometric and algebraic methods. The final solution is an explanatory formula for the robot’s joint position variables. These methods have different specific calculation processes for different robots. Moreover, robots must meet the condition that there exists an analytical solution for the robot’s inverse kinematics. However, not every robot’s inverse kinematics can be solved analytically. So alternative methods are required for these cases. The Jacobi method at the speed level has the same inverse kinematics calculation process for different robots and does not require analytical solutions for the inverse kinematics of the robot. It is a universal inverse kinematics solution method. Compared to the position-level solution method, the disadvantage of the Jacobi method is its higher computational cost, which includes the calculation of the Jacobi matrix, leading to slower execution. However, its advantage lies in its broader applicability. The performance requirements for the Microcontroller Unit (MCU) are higher, so MCU chips with faster clock frequencies are necessary [35,36].

The Jacobi matrix represents the relationship between the rotational speed of the robot’s joints and the linear and angular velocities at the end effector of the robot. Various methods exist for calculating the Jacobi matrix, primarily including the method of differentiating the pose equation, the recursive link speed method, and the vector multiplication and differential transformation method. This article focuses on the vector multiplication method, which constructs the Jacobi matrix through link velocity analysis [37].

Industrial robots use rotary joints, so this article introduces the Jacobi matrix of rotary joints. For the rotating joint, i, the linear velocity and angular velocity are as follows:

$$\left[\begin{array}{c}{v}_i\\ {\omega }_i\end{array}\right]=\left[\begin{array}{c}{z_i\times }^i{p}_n^0\\ z_i\end{array}\right]{\dot{q}}_i$$

(16)

In Equation (16), $z_i$ is the unit vector of the robot’s i-joint in the z-direction, which is the third column of the ${}_6{}^{i}T$ matrix. ${\dot{q}}_i$ is the derivative of the i-joint angle, and ⁱ$p_n^0$ is the position vector of the robot’s end effector origin relative to the coordinate system {i} in the base coordinate system {0}. Therefore, the Jacobi matrix of joint i is:

$$J_i=\left[\begin{array}{c}{\mathrm{J}}_{vi}\\ {\mathrm{J}}_{\omega i}\end{array}\right]=\left[\begin{array}{c}{z_i\times }^i{p}_n^0\\ z_i\end{array}\right]$$

(17)

$$p_n^0={}_i{}^0{R}^i{p}_n$$

(18)

where ${}_i{}^{0}R$ represents the attitude transformation matrix between the joint coordinate system {i} and the base coordinate system {0}, and ⁱ$p_n$ is the position vector of the robot’s end-effector in the coordinate system {i}.

For a six-joint robot, the Jacobi matrix constructed by vector cross multiplication is as follows:

$$\begin{gathered} J\left(q\right)=\left[\begin{array}{c}{z_1\times }^1{p}_6^0\\ z_1\end{array} \begin{array}{c}{z_2\times }^2{p}_6^0\\ z_2\end{array} \begin{array}{c}\dots \\ \dots \end{array} \begin{array}{c}{z_6\times }^6{p}_6^0\\ z_6\end{array}\right] \\ =[J_1 J_2 J_3 J_{4 }{J}_5 J_6] \end{gathered}$$

(19)

Therefore, the linear velocity and angular velocity at the end of the robot can be expressed as follows:

$$\left[\begin{array}{c}{v}_6\\ {\omega }_6\end{array}\right]=\left[\begin{array}{c}{z_1\times }^1{p}_6^0\\ z_1\end{array} \begin{array}{c}{z_2\times }^2{p}_6^0\\ z_2\end{array} \begin{array}{c}\dots \\ \dots \end{array} \begin{array}{c}{z_6\times }^6{p}_6^0\\ z_6\end{array}\right]\left[\begin{array}{c}\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\\ {\dot{q}}_4\\ {\dot{q}}_5\\ {\dot{q}}_6\end{array}\right]$$

(20)

For a six-degree-of-freedom industrial robot, the following relationship holds:

$${\left[{\dot{q}}_1 {\dot{q}}_2 {\dot{q}}_3 {\dot{q}}_4 {\dot{q}}_5 {\dot{q}}_6\right]}^T={\left[{\dot{\theta }}_1 {\dot{\theta }}_2 {\dot{\theta }}_3 {\dot{\theta }}_4 {\dot{\theta }}_5 {\dot{\theta }}_6\right]}^T$$

(21)

Therefore, Equation (20) can be written as follows:

$$\left[\begin{array}{c}{v}_6\\ {\omega }_6\end{array}\right]=\left[J_1 J_2 J_3 J_{4 }{J}_5 J_6\right]\left[\begin{array}{c}\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\\ {\dot{\theta }}_3\end{array}\\ {\dot{\theta }}_4\\ {\dot{\theta }}_5\\ {\dot{\theta }}_6\end{array}\right]$$

(22)

3.2.2. Robot Body Dynamics Modeling

The key to the dynamic analysis of a robot’s body is to establish the dynamic equations for industrial robots and to solve for the various parameters in these equations. This article uses the Euler–Lagrange method to derive the dynamic equations. For a single degree of freedom system with complete constraints, using the Euler–Lagrange approach and beginning with the virtual work concept, the Euler–Lagrange equation can be derived [38].

$${\displaystyle \frac{d}{dt}}{\displaystyle \frac{\mathit{\partial }L}{\mathit{\partial }\dot{y}}}-{\displaystyle \frac{\mathit{\partial }L}{\mathit{\partial }y}}=f$$

(23)

In Equation (23), y represents the generalized coordinate, and $\dot{y}$ is the generalized velocity. The symbol L is the Lagrange operator, which represents the difference between the kinetic energy and potential energy of the system: L = K − P, where K is kinetic energy and P is potential energy; in Equation (23), $f$ represents the generalized force acting on the system, excluding gravity. Applying Equation (23) to industrial robot systems, the Euler–Lagrange equation of the robot can be obtained:

$${\displaystyle \frac{d}{dt}}{\displaystyle \frac{\mathit{\partial }L}{\mathit{\partial }\dot{{\theta }_i}}}-{\displaystyle \frac{\mathit{\partial }L}{\mathit{\partial }\theta }}={\tau }_i$$

(24)

In Equation (24), ${\theta }_i$ is a generalized coordinate representing the angle of the robot joint, ${\dot{\theta }}_i$ is the acceleration of robot joint i, and ${\tau }_i$ represents the torque applied to the connecting rod of the corresponding joint.

Next, we calculate the Lagrange operator, L, starting with the kinetic energy. Translational and rotational kinetic energy make up the two components of the robot’s joint kinetic energy. For simplicity, the mass of the connecting rod is concentrated at the mass position’s center. Therefore, the kinetic energy of the link is as follows:

$$K={\displaystyle \frac{1}{2}}{m}_i{v}_i^T{v}_i+{\displaystyle \frac{1}{2}}{\omega }_i^T{I}_i^0{\omega }_i$$

(25)

In Equation (25), $m_i$ represents the mass of the connecting link, i; $v_i$ and ${\omega }_i$, respectively, represent the linear velocity and angular velocity of the robot link, i; and $I_i^0$ represents the inertia tensor of the link, i, relative to the base coordinate system {0}. The position of the joint, i, relative to the base coordinate system is generally uncertain. For ease of calculation, the inertia tensor, $I_i$, of the link, i, relative to the joint coordinate system can be calculated first and then transformed into the inertia tensor, $I_i^0$, of the link, i, relative to the base coordinate system. The conversion formula is as follows:

$$I_i^0={}_i{}^{0}R{I}_i{{}_i{}^{0}R}^T$$

(26)

In Equation (26), ${}_i{}^{0}R$ represents the rotation matrix between the joint coordinate system, i, and the base coordinate system. The inertia tensor, $I_i$, is a constant matrix that is independent of joint motion.

$$I_i=\left[\begin{array}{ccc}{I}_{XX}& I_{XY}& I_{XZ}\\ I_{YX}& I_{YY}& I_{YZ}\\ I_{ZX}& I_{ZY}& I_{ZZ}\end{array}\right]$$

(27)

In Equation (27),

$$\left\{\begin{array}{c}{I}_{XX}=\iiint (y^2+z^2)\rho (x,y,z)dxdydz\\ I_{YY}=\iiint (x^2+z^2)\rho (x,y,z)dxdydz\\ I_{ZZ}=\iiint (x^2+y^2)\rho (x,y,z)dxdydz\\ I_{XY}=I_{YX}=-\iiint xy\rho (x,y,z)dxdydz\\ I_{XZ}=I_{ZX}=-\iiint xz\rho (x,y,z)dxdydz\\ I_{YZ}=I_{ZY}=-\iiint yz\rho (x,y,z)dxdydz\end{array}\right.$$

(28)

$\rho (x,y,z)$ is the density of the link, and x, y, and z are the coordinates of the link. Due to the complexity of directly calculating the inertia tensor, $I_i$, this paper uses 3D modeling and Solidworks software (2023 version) to analyze and calculate the inertia tensor of the link, i, relative to the joint coordinate system [39].

Because $v_i=J_{v_i}\left(\theta \right)\dot{\theta }$, ${\mathsf{\omega }}_i=J_{{\mathsf{\omega }}_i}\left(\theta \right)\dot{\theta }$

$$\begin{gathered} K={\displaystyle \frac{1}{2}}{m}_i{v}_i^T{v}_i+{\displaystyle \frac{1}{2}}{\omega }_i^T{I}_i^0{\omega }_i \\ ={\displaystyle \frac{1}{2}}{\dot{\theta }}^T\left\{\sum _{i=1}^6\left[m_i{J}_{v_i}{\left(\theta \right)}^T{J}_{v_i}\left(\theta \right)+m_i{J}_{{\omega }_i}{\left(\theta \right)}^T{R}_i^0{I}_i{R}_i^{0T}{J}_{{\omega }_i}\left(\theta \right)\right]\right\}\dot{\theta }={\displaystyle \frac{1}{2}}{\dot{\theta }}^{T}D\left(\theta \right)\dot{\theta } \end{gathered}$$

(29)

In Equation (29),

$$D\left(\theta \right)=\sum _{i=1}^6\left[m_i{J}_{v_i}{\left(\theta \right)}^T{J}_{v_i}\left(\theta \right)+m_i{J}_{{\omega }_i}{\left(\theta \right)}^T{R}_i^0{I}_i{R}_i^{0T}{J}_{{\omega }_i}\left(\theta \right)\right]$$

(30)

$D\left(\theta \right)$ is the inertia matrix of the robot, which is a symmetric matrix and positive definite.

Let

$$\left[d_{ij}\right]=\left[m_i{J}_{v_i}{\left(\theta \right)}^T{J}_{v_i}\left(\theta \right)+m_i{J}_{{\omega }_i}{\left(\theta \right)}^T{R}_i^0{I}_i{R}_i^{0T}{J}_{{\omega }_i}\left(\theta \right)\right]$$

(31)

Equation (29) can be written as follows:

$$K={\displaystyle \frac{1}{2}}\sum _{i,j}^6{d}_{ij}\left(\theta \right){\dot{\theta }}_i {\dot{\theta }}_j$$

(32)

d_ij is an element in the inertia matrix, $D\left(\theta \right)$, and the Lagrange operator can be transformed into the following:

$$L=K-P={\displaystyle \frac{1}{2}}\sum _{i,j}^6{d}_{ij}\left(\theta \right){\dot{\theta }}_i {\dot{\theta }}_j-P(\theta )$$

(33)

By substituting Equation (32) into the Lagrange Equation (24), we can obtain the following:

$$\sum _j{d}_{ij}{\ddot{\theta }}_j+{\displaystyle \frac{1}{2}}\sum _{i,j}\left[{\displaystyle \frac{{\mathit{\partial }d}_{kj}}{{\mathit{\partial }\theta }_i}}+{\displaystyle \frac{{\mathit{\partial }d}_{kj}}{{\mathit{\partial }\theta }_i}}-{\displaystyle \frac{{\mathit{\partial }d}_{ij}}{{\mathit{\partial }\theta }_k}}\right]{\dot{\theta }}_i {\dot{\theta }}_j+{\displaystyle \frac{\mathit{\partial }P}{{\mathit{\partial }\theta }_k}}={\tau }_k$$

(34)

In Equation (34), the servo can calculate $d_{ij}$ by setting various dynamic parameters and sampling the encoder position in real time, while ${\theta }_i$ is the encoder position, which can be sampled in real time. Using these data, the torque for each axis can be calculated using the above equation [40].

The potential energy, P, of the robot is considered next. Since the industrial robot used in this paper is a rigid robot, the potential energy only needs to account for gravitational potential energy. Assuming the mass of the joint is centered at the mass center the gravitational potential energy of the link, i, is given by the following:

$$P_i=m_i{g}^T{r}_{ci}$$

(35)

In Equation (35), g is the gravity vector in the base coordinate system {0}; $r_{ci}$ represents the coordinates of the centroid of the joint, i, in the base coordinate system. The total potential energy of the robot is as follows:

$$P=\sum _{i=1}^6{P}_i=\sum _{i=1}^6{m}_i{g}^T{r}_{ci}$$

(36)

Therefore, Equation (34) can be written as:

$$\sum _{j=1}^6{d}_{jk}{\ddot{\theta }}_j+{\displaystyle \frac{1}{2}}\sum _{i,j}\left[{\displaystyle \frac{{\mathit{\partial }d}_{kj}}{{\mathit{\partial }\theta }_i}}+{\displaystyle \frac{{\mathit{\partial }d}_{kj}}{{\mathit{\partial }\theta }_i}}-{\displaystyle \frac{{\mathit{\partial }d}_{ij}}{{\mathit{\partial }\theta }_k}}\right]{\dot{\theta }}_i{\dot{\theta }}_j+\sum _{i=1}^6{m}_i{g}^T{\displaystyle \frac{\mathit{\partial }{r}_{ci}}{{\mathit{\partial }\theta }_k}}={\tau }_i$$

(37)

The torque of axes 1–6 can be calculated in real time using Equation (37). In practical engineering applications, due to the servo’s sampling rate for encoder positions and the speed at which calculations are performed, the torque calculation algorithm is often implemented directly within the servo to enhance computation speed.

Because the robot tools influence the six-axis dynamic model, it is necessary to model each tool individually to determine its centroid position, mass, and calculate the moment of inertia. As a result, even for the same robot, variations in the tool load will lead to differences in the center of mass and mass distribution across the six axes. The tool’s characteristics significantly affect the calculated torque and the feedback torque of each axis of the robot [41].

Table 3. Robot dynamic collision detection parameter file.

Name	Joint 1	Joint 2	Joint 3	Joint 4	Joint 5	Joint 6	Tool
Joint mass (kg)	221.74	66.463	149.58	75.778	19.3	0	50
Centroid coordinate X (m)	−0.14956	−0.58	−0.1364	0.00488	0.001234	0.1	0.25
Centroid coordinate Y (m)	−0.0663	−0.00008	−0.013851	−0.4611	0.0009	0	0.001
Centroid coordinate Z (m)	kg·m2	0.1198	0.046264	−0.0514	0.0593	0.18	0.18
$I_{XX}$ (kg·m2)	5.0181	0.62859	2.6286	7.2478	0.1718	5	0.01
$I_{YY}$ (kg·m2)	10.079	11.092	3.1253	7.0028	0.08182	5	0.01
$I_{ZZ}$ (kg·m2)	11.056	0.62859	2.6286	7.2478	0.1718	3	0.01
$I_{XY}$ (kg·m2)	0	0	0	0	0	0	0
$I_{YZ}$ (kg·m2)	0	0	0	0	0	0	0
$I_{XZ}$ (kg·m2)	0	0	0	0	0	0	0
$I_{XX}$ (kg·m2)	0	0	0	0	0	0	0
Motor Rated torque (N·m)	26.3	26.3	14.33	4.8	4.8	3.18	0
Motor rated current (A)	24	24	7.6	4.8	4.8	3.5	0
Reduction ratio	210	219.46	145	133.59	157.33	105	1
Coulomb friction force (N·m)	220	574.45	250	40	97.837	33.687	0
Kinetic friction coefficient	530	874.53	250	80	55.84	27.4	0

displays the values needed for dynamic collision detection.

The parameters required for dynamic collision detection are shown in Table 3, including joint mass, centroid position, moment of inertia, motor rated current, motor rated torque, and other parameters. The inertia moment parameter is calculated through mechanical software (Solidworks 2023 version).

The dynamic modeling approach indeed increases computational overhead but does not affect control performance. In this study, the highest priority is given to motor current sampling, encoder sampling, and motor control, followed by fault protection mechanisms such as overcurrent detection. The third priority is the torque calculation for dynamic modeling. The system utilizes a DSP chip with a 200 MHz clock frequency, and the entire dynamics-based collision detection computation takes approximately 275 microseconds, which satisfies real-time requirements. Employing a higher-frequency chip would further reduce computation time and enhance real-time performance.

In reality, robotic systems exhibit nonlinear characteristics, such as those introduced by inverter nonlinearities and dead-time effects. These nonlinearities can be effectively mitigated through the implementation of compensation strategies.

Additionally, friction compensation plays a crucial role in enhancing the accuracy of calculated torque. Friction torque is typically obtained through experimental measurements, and motor manufacturers often provide these data. However, it is common practice to experimentally verify the friction torque parameters supplied by manufacturers. The parameter table has been updated to include both Coulomb friction and the coefficient of kinetic friction. In practical calculations, if the coefficient of kinetic friction is non-zero, it will be factored into the theoretical torque computations. Both Coulomb friction and the coefficient of kinetic friction have been integrated into the dynamic calculations.

Regular maintenance and proper lubrication are critical for reducing frictional torque. Although the Coulomb friction of the motor remains relatively stable, it may undergo changes during prolonged operation. Variations in speed can affect viscous friction, thereby altering the frictional torque. Over extended periods, wear on components such as bearings and gears can modify the contact surface conditions, influencing the frictional torque. Additionally, the aging, contamination, or insufficient application of lubricants can increase frictional torque, and the performance differences among various lubricants further contribute to these effects. In this study, static friction torque and kinetic friction coefficients are tabulated and transmitted to the servo system to compensate for the theoretical torque. It is important to note that frictional parameters vary across different joints. Furthermore, both the motor and the robot are susceptible to thermal effects, and incorporating a temperature compensation factor into the theoretical torque calculations is recommended to account for these influences.

Table 4. The relationship between the threshold value and the success rate of collision detection, false alarm rate, and missed detection rate.

Threshold (%)	Success Rate (%)	False Alarm Rate (%)	Miss Rate (%)
105	100	20	0
110	100	2	0
115	100	0	0
120	100	0	0

shows the relationship between the threshold value and the success rate of collision detection, false alarm rate, and missed detection rate.

Owing to the inherent discrepancies between the theoretical torque and the feedback torque, the threshold value must not be established too minimally. With the threshold calibrated at 105%, each collision was promptly identified; however, this configuration was accompanied by a significant false positive rate. Specifically, during ten operational cycles of the robot, collisions were erroneously reported in the complete absence of any physical impact. While the 105% threshold ensured the detection of all collisions, the elevated rate of false positives rendered this threshold inappropriate. Consequently, the threshold was recalibrated to 110%. At this juncture, collisions continued to be detected with alacrity, yet the false positive rate persisted, manifesting as two spurious collision reports over 100 operational cycles devoid of any actual collisions. Upon further adjustment of the threshold to 115%, and after several hundred test cycles without any collisions, no false positives were recorded. Thresholds of 105%, 110%, 115%, and 120% were subjected to empirical testing by introducing an obstacle for the robot to collide with, and all collisions were accurately reported without fail. Thus, it is empirically validated that establishing the threshold for actual collision detection at 120% of the maximum deviation between the theoretical and calculated torque is judicious for practical implementation.

The threshold value is set to 1.2, which is obtained through experiments, and a certain margin is reserved. Because the theoretical torque and feedback torque cannot be completely consistent and synchronized, there are errors between them. Theoretically, if all factors are considered carefully, the error between the theoretical torque and the feedback torque will be very small, so the threshold value should be about 1.05, and the collision can be detected faster. But in the actual operation process, we found through experiments that even if all parameters are verified, there are still errors in the theoretical torque and the calculated torque, especially in the acceleration and deceleration phases, and the errors are more difficult to be adjusted through parameters. If it is found in the experiment that the deviation between the theoretical torque and the actual torque is too large during the normal operation of the robot, a parameter for calculating the theoretical torque may not be accurate, or some factors are not taken into account. It is necessary to adjust the parameters to make the theoretical torque and feedback torque as consistent as possible. But even so, with the increase in running time, the deviation between the theoretical torque and the actual torque will become larger, which is mainly because the feedback torque may be different from the initial running stage with the change in friction, temperature, lubrication, and other factors of the robot. In the experiment, even if the threshold is set to 1.15, all collisions can be successfully detected without false detection. The reason for setting it to 1.2 is that the deviation between theoretical torque and feedback torque will increase due to long-term operation, and the threshold value will be enlarged to 1.2. The advantage of scaling up to 1.2 is that it can avoid false alarms and detect them smoothly, but the disadvantage is that the detection time will be a little slow.

The industrial robot, which includes dynamic collision detection software (Servo v1.10), has undergone MTBF testing at an authoritative third-party testing institution in China and has passed 80,000 h of MTBF testing, proving that the system has high stability.

4. Experiment

4.1. The Situation During Normal Operation

In the actual testing, a robot with a 50 kg load was used. When there was no collision, i.e., during normal operation, the waveforms of the calculated torque and feedback torque for each axis are shown in Figure 6. The feedback torque is shown by the blue line, while the red line indicates the calculated torque or theoretical torque. The feedback torque and calculated torque of six joints of the robot are sampled in the figure. It is evident that the trajectory of the calculated torque closely matches that of the feedback torque, with the difference between them remaining small. The calculated torque is based on the encoder angle, velocity, and acceleration, whereas the actual torque is derived from motor current sampling. The red line in the figure represents the calculated torque, and the feedback torque is shown by the blue line.

The threshold value is automatically set, and without activating the collision detection function, the robot completes a complete motion once. Find the maximum value Δ of the difference between the calculated torque and the feedback torque throughout the entire motion process. Set 1.2 $\times$Δ as the threshold value and then activate the collision detection function. This task will be automatically completed by the servo. That is to say, once collision detection is enabled, the first action is to find the collision threshold value. Subsequently, the collision detection function only truly takes effect with the repeated actions of the robot.

4.2. X-Axis Collision Experiment

Figure 7 illustrates the scenario of when a collision occurs along the X-axis. While the robot moves along the X-axis, joints 2, 3, and 5 are in motion. When a collision occurs in the X-axis direction, there is a sudden change in the feedback torque and calculated torque for joints 2, 3, and 5. The changes are particularly pronounced in joints 2 and 3. During a collision, the distinction between the feedback torque and the calculated torque exceeds a predefined threshold, triggering a rapid shutdown to protect the robot from damage. Figure 8a,d,g represents the calculated torque and feedback torque of each joint of the robot during normal operation along the X-axis. Figure 8b,e,h represents the moments when the robot collided while running along the X-axis, resulting in a sudden change in the calculated torque and feedback torque of each joint. Immediately stop the machine after a collision occurs, so the calculated torque and feedback torque values after the collision are both equal to 0. Figure 8c,f,i shows the enlarged waveforms of the calculated torque and feedback torque for each joint at the moment of collision. In these enlarged sections, the computed torque is shown by the red line, while the feedback torque is shown by the blue line.

4.3. Y-Axis Collision Experiment

Figure 9 shows the scenario of when a collision occurs along the Y-axis. During this operation, when the robot moves along the Y-axis, joints 1, 2, 3, 5, and 6 all experience motion.

Figure 10a,d,g,j,m, shows the calculated torque and feedback torque of each joint during normal operation of the robot along the Y-axis. Figure 10b,e,h,k,n show that a collision occurred at a certain moment when the robot was running along the Y-axis, resulting in a sudden change in the calculated torque and feedback torque of each joint. Figure 10c,f,i,l,o show the enlarged waveforms of the calculated torque and feedback torque details for each joint at the moment of collision.

4.4. Z-Axis Collision Experiment

When a collision happens along the robot’s Z-axis, the torque changes in joint 2 and joint 3 are the most significant. Figure 11 illustrates the scenario of when a collision happens along the Z-axis. During this operation, when the robot moves along the Z-axis, joints 2, 3, and 5 experience motion.

Figure 12a,d,g shows the calculated torque and feedback torque during normal operation of the robot along the Z-axis. Figure 12b,e,h shows that a collision occurred at a certain moment when the robot was running along the Z-axis, resulting in a sudden change in the calculated torque and feedback torque of each joint. Figure 12c,f,i shows enlarged waveforms of the calculated torque and feedback torque details for each joint at the moment of collision.

5. Conclusions

This paper utilizes a servo drive dynamic detection module to enable collision detection for industrial robots. Although the dynamic calculations involved are relatively large and time-consuming, collision detection has been successfully implemented in the system, which is crucial for protecting the robot from severe damage. The experimental findings show that the servo drive can precisely and quickly detect a collision through dynamic calculation. When a sudden change in the feedback torque and the deviation from the calculated torque is greater than the set collision threshold value, it can be detected quickly. It can immediately release the brake of the motor and stop the servo from working. This will help protect robots from serious collision injuries. The proposed collision detection solution is low-cost and can, theoretically, handle multiple collisions without rendering the robot inoperable. This approach provides valuable insights for future research. However, further research is required to increase the precision of collision detection based on dynamic calculations and to enhance the speed of collision detection for even quicker response times.