Robot joint current and torque cooperative monitoring collision detection method and system

Abstract

The invention discloses a robot joint current and torque cooperative monitoring collision detection method and system, and relates to the technical field of robots. Joint body disturbance and tail end real collision can be accurately distinguished by fusing each joint state, a motor current observation value and a base torque value; interference caused by load change or high-speed acceleration and deceleration is effectively filtered out; secondly, a time-varying safety confidence coefficient function based on historical statistical characteristics of a torque observation value is introduced, so that a collision detection threshold value can be adaptively adjusted along with the real-time working condition of the robot, and the problems that a fixed threshold value is prone to missing report under high-speed heavy load and prone to false report under low-speed light load are solved; and finally, the dynamic threshold value and the real-time torque deviation are compared one by one, collision joints can be accurately positioned at the moment of collision, an alarm is given out, precious time is won for rapid and safe response of the robot, and the safety of man-machine cooperation is greatly improved.

Description

Technical Field

[0001] This application relates to the field of robotics technology, specifically to a method and system for collision detection by coordinated monitoring of robot joint current and torque. Background Technology

[0002] In robotics applications, especially collaborative robots, collision detection is a key technology for achieving safe human-robot interaction. Traditional collision detection methods typically rely on a single current threshold or simple torque deviation monitoring. However, these methods have several limitations: fixed thresholds are difficult to adapt to the large-scale torque fluctuations under high-speed robot motion or variable load conditions, easily leading to false alarms; relying solely on joint data cannot effectively distinguish between collisions caused by the end effector's contact with the environment and interference from changes in joint internal friction; furthermore, traditional methods often lack statistical analysis of historical data and cannot adaptively adjust detection sensitivity based on the robot's real-time operating status. Therefore, existing collision detection technologies suffer from detection lag, insufficient sensitivity, or frequent false triggers when handling complex tasks, variable loads, and sudden end-effector collisions, making it difficult to meet the dual requirements of high safety and high productivity.

[0003] Existing technology, such as the invention application patent with publication number CN113021353A, discloses a robot collision detection method applicable to robot systems consisting of at least one robot joint, a servo driver, a current detector, and a controller. The robot joint includes a link, a reducer, and a servo motor. The servo motor is equipped with a position detection unit that detects the robot joint position information in real time, and the current detector detects the servo motor drive current in real time. Utilizing the principle that the robot joint motor current I can be decomposed into a motor current component I1 for achieving dynamic motion, a motor current component I2 for overcoming joint friction, and a motor current component I3 caused by collision, when the motor current component I3 caused by collision exceeds a collision threshold, it is determined that the robot joint link has collided. The robot collision detection method proposed in this invention can detect collisions between the robot and its external environment without the need for additional sensors.

[0004] Regarding the above-mentioned solutions, the inventors of this application have discovered that the above-mentioned technology has at least the following technical problems:

[0005] 1. Existing technologies typically do not employ robot dynamics models to predict theoretical torque and lack dynamic, collision-free reference benchmarks. Therefore, they struggle to adapt to load variations under different robot postures and speeds, often relying on fixed thresholds for collision detection. When the robot moves at high speeds, fixed thresholds easily misinterpret normal motion as collisions, leading to frequent false alarms; while under low-speed, heavy-load conditions, the torque changes caused by collisions are not significant, easily resulting in missed alarms, leading to poor overall detection accuracy.

[0006] 2. Existing technologies lack a mechanism to cross-verify collision results of each joint using the base torque as a global reference system, typically relying solely on torque changes in a single joint to determine a collision. When a joint is subjected to vibration interference or malfunctions, false alarms are easily generated, making it difficult to effectively distinguish between genuine collisions and interference signals. Due to the lack of a "global-local" verification mechanism, the reliability and anti-interference capability of collision event confirmation are low.

[0007] 3. Existing collision detection thresholds often rely on fixed empirical values, which cannot be dynamically adjusted to adapt to changing operating conditions. When robot wear occurs, load changes, or ambient temperature affects current characteristics, fixed thresholds struggle to adapt to these changes, leading to decreased sensitivity or even failure. Furthermore, the lack of a learning mechanism based on historical statistical characteristics and collision confirmation markers prevents adaptive threshold drift. In addition, existing technologies typically employ rigid judgments, failing to provide probabilistic levels of safety confidence and thus unable to provide a basis for subsequent decision-making. Summary of the Invention

[0008] To address the aforementioned technical shortcomings, the purpose of this application is to provide a method and system for collaborative monitoring of robot joint current and torque, and collision detection.

[0009] To solve the above-mentioned technical problems, this application adopts the following technical solution: In the first aspect, this application provides a collision detection method for joint current and torque collaborative monitoring of a robot. The method includes the following steps: S1, real-time acquisition of the state data of each joint of the robot, the current vector of the drive motor of each joint and the torque value of the base.

[0010] S2. Calculate the torque observation value of each joint based on the drive motor current vector of each joint.

[0011] S3. Based on the state data of each joint, calculate the predicted torque value of each joint under collision-free conditions using the robot inverse dynamics model.

[0012] S4. Based on the torque observation values ​​and torque prediction values ​​of each joint, the torque observation value vector of each joint is obtained; and based on the base torque value, the end-collision result is analyzed to generate the collision confirmation mark of each joint.

[0013] S5. Based on the historical statistical characteristics of the torque observation vector of each joint and the collision confirmation identifier of each joint, analyze and obtain the time-varying safety confidence function value of each joint, and generate the collision detection threshold of each joint.

[0014] S6. Based on the collision confirmation flags of each joint and the dynamically generated collision detection thresholds of each joint, determine whether a collision has occurred.

[0015] Preferably, the joint state data includes the robot's joint angle vector, joint angular velocity vector, and joint angular acceleration vector.

[0016] Preferably, the calculation of the torque observation value of each joint based on the drive motor current vector of each joint includes: calculating the torque observation value of each joint using a linear mapping formula based on the drive motor current vector and the torque constant vector of each joint. In conclusion Time of day Torque observations of each joint ,in This is represented by the number corresponding to each joint. , This represents the total number of joints. Indicated as in Time of day The torque constant vector of the motor at each joint Indicated as in Time of day The current vector of the drive motor for each joint.

[0017] Preferably, the step of calculating the torque prediction value of each joint under collision-free conditions based on the state data of each joint using the robot inverse dynamics model includes: S401, analyzing and obtaining the Coriolis and centrifugal torque vectors based on the joint angle vector and joint angular velocity vector.

[0018] S402, Based on the joint angle vector, obtain the gravitational moment vector corresponding to the current robot configuration from the robot inverse dynamics model database.

[0019] S403, based on the joint angular velocity vector, obtain the friction torque vector corresponding to the current joint motion speed from the robot inverse dynamics model database.

[0020] S404, perform vector addition on the inertial torque vector, the Coriolis and centrifugal torque vector, the gravitational torque vector and the frictional torque vector to obtain the torque prediction value of the joint under the condition of no external collision, and thereby obtain the torque prediction value of each joint.

[0021] Preferably, the step of analyzing and deriving the Coriolis and centrifugal torque vectors based on the joint angle vector and joint angular velocity vector includes: S501, obtaining the mass matrix corresponding to the current robot configuration from a pre-built robot inverse dynamics model database based on the joint angle vector.

[0022] S502, based on the joint angular velocity vector, joint angular acceleration vector and mass matrix, perform matrix multiplication to calculate the inertial force components and obtain the inertial torque vector.

[0023] S503, based on the joint angle vector and joint angular velocity vector, obtain the Coriolis and centripetal torque matrices corresponding to the current robot configuration and motion speed from the robot inverse dynamics model database.

[0024] Based on the joint angle vector and joint angular velocity vector, S504 analyzes and obtains the Coriolis force and centrifugal force components, and then obtains the Coriolis and centrifugal torque vectors.

[0025] Preferably, the step of deriving the torque observation vector for each joint based on the observed torque values ​​and predicted torque values ​​for each joint includes: calculating the torque vector using a formula. In conclusion Time of day Torque deviation vector of each joint ,in Indicated as in Time of day The vector of torque observations for each joint. Indicated as in Time of day Torque prediction values ​​for each joint.

[0026] Preferably, the step of analyzing the end-effector collision results based on the base torque value and generating collision confirmation flags for each joint includes: performing a mapping operation from torque to joint torque based on the base torque value and the robot Jacobian matrix determined by the angle vector of the current joint to obtain the spatial equivalent torque of each joint; and then performing a collision confirmation judgment to generate a collision confirmation flag for each joint.

[0027] Preferably, the analysis yields the time-varying safety confidence function value of each joint and generates the collision detection threshold for each joint, including: S801, calculating the statistical characteristics characterizing the degree of abnormality based on the historical data of the joint torque deviation vector within a preset time window.

[0028] S802, based on the statistical characteristics and the collision confirmation flag, the safety confidence function value of each joint at the current moment is calculated using a time-varying safety confidence function.

[0029] S803, based on the safety confidence function value and the preset default static collision detection threshold, the time-varying collision detection threshold of each joint is dynamically generated through proportional scaling calculation.

[0030] Preferably, the step of determining whether a collision has occurred based on the collision confirmation flags of each joint and the dynamically generated collision detection thresholds of each joint includes: S901, obtaining the input data required for collision decision based on the torque deviation vectors of each joint and the dynamic collision detection thresholds of each joint.

[0031] S902, based on the torque deviation components of each joint and the corresponding time-varying collision detection threshold, performs an absolute value comparison operation to generate a Boolean logic flag characterizing whether the joint has abnormally exceeded the limit.

[0032] S903 performs a logical "OR" operation based on the Boolean logic flags of all joints to determine the global collision state, and generates and outputs a collision alarm signal and collision joint position identifier based on the determination result.

[0033] In its second aspect, this application provides a system for a robot joint current and torque collaborative monitoring and collision detection method, comprising: a data acquisition module for acquiring in real time the state data of each joint of the robot, the drive motor current vector of each joint, and the base torque value.

[0034] The torque observation calculation module calculates the torque observation value of each joint based on the drive motor current vector of each joint.

[0035] The torque prediction calculation module calculates the torque prediction value of each joint under collision-free conditions based on the state data of each joint and through the robot inverse dynamics model.

[0036] The collision confirmation marker generation module derives the torque observation value vector for each joint based on the torque observation value and the torque prediction value of each joint; and analyzes the end-collision result based on the base torque value to generate a collision confirmation marker for each joint.

[0037] The collision detection threshold generation module analyzes and derives the time-varying safety confidence function value of each joint based on the historical statistical characteristics of the torque observation vector of each joint and the collision confirmation identifier of each joint, and generates the collision detection threshold of each joint.

[0038] The collision determination module determines whether a collision has occurred based on the collision confirmation flags of each joint and the dynamically generated collision detection thresholds for each joint.

[0039] The beneficial effects of this application are as follows: 1. The robot joint current and torque collaborative monitoring collision detection method and system provided in this application can accurately distinguish between joint body disturbances and actual end-effector collisions by integrating the joint states, motor current observations, and base torque values, effectively filtering out interference caused by load changes or high-speed acceleration and deceleration; secondly, by introducing a time-varying safety confidence function based on the historical statistical characteristics of torque observations, the collision detection threshold can be adaptively adjusted according to the robot's real-time operating conditions, solving the problem that fixed thresholds are prone to missed detections under high-speed heavy loads and prone to false detections under low-speed light loads; finally, by comparing the dynamic threshold with the real-time torque deviation joint by joint, the collision joint can be accurately located and an alarm can be issued at the moment of collision, gaining valuable time for the robot's rapid and safe response and greatly improving the safety of human-robot collaboration.

[0040] 2. This application utilizes the robot's dynamic model to calculate the theoretically expected torque based on the current motion state (position, velocity, acceleration). This provides an ideal "collision-free" reference benchmark. The predicted value can dynamically adapt to load variations under different robot postures and speeds, avoiding false alarms (such as being misjudged as collisions during high-speed motion) or missed alarms (such as insignificant collisions during low-speed heavy loads) caused by using fixed thresholds, significantly improving detection accuracy.

[0041] 3. This application introduces, for the first time, the base torque value as a global reference system to cross-validate the collision results of each joint, thereby generating a "collision confirmation flag." This effectively solves the problem of false alarms from single joints. For example, when the robot's end effector collides with an object, the base torque will inevitably change; if a joint shows a collision but the base torque does not respond, it can be determined that the joint itself is faulty or subject to vibration interference. This "global-local" verification mechanism greatly improves the reliability and anti-interference capability of collision event confirmation.

[0042] 4. This application features dynamic threshold generation and time-varying confidence. It abandons fixed empirical thresholds and generates dynamic thresholds and a safety confidence function that change with operating conditions by analyzing historical statistical characteristics and learning from collision confirmation markers. This achieves adaptive threshold adjustment. When robot wear, load changes, or ambient temperature affects current characteristics, the threshold can drift and adjust in real time according to historical statistical characteristics, always maintaining optimal sensitivity. The safety confidence function provides a probabilistic reference for subsequent decisions, avoiding rigid, one-size-fits-all judgments. Attached Figure Description

[0043] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0044] Figure 1 This is a flowchart illustrating the steps involved in implementing the method described in this application.

[0045] Figure 2 This is a schematic diagram of the system structure connection of this application. Detailed Implementation

[0046] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0047] Please see Figure 1 As shown, this application provides a collision detection method for coordinated monitoring of robot joint current and torque in its first aspect, including:

[0048] S1. Real-time acquisition of the status data of each joint of the robot, the current vector of the drive motor of each joint, and the torque value of the base;

[0049] In a specific example, the state data of the joint includes the robot's joint angle vector, joint angular velocity vector, and joint angular acceleration vector.

[0050] It should be noted that, based on the robot's servo encoder, the robot's real-time motion state is measured to obtain joint angle vectors, joint angular velocity vectors, and joint angular acceleration vectors; based on the drive motors of each joint, the real-time operating current of the motors is sampled to obtain joint current vectors; based on the six-dimensional force and torque sensors installed on the robot base, the force and torque in the robot base coordinate system are measured to obtain the base torque value.

[0051] It should be noted that measuring the robot's real-time motion state refers to continuously reading the raw angular displacement signal of the motor shaft through a high-precision photoelectric or magnetic encoder in each joint servo system, and based on the time series of the signal, performing first and second differentiation processing through a hardware or firmware-level differential calculation unit, thereby converting the angular position information into angular velocity and angular acceleration information.

[0052] It should be noted that the joint angle vector is an n-dimensional column vector describing the spatial position of each joint of the real-time robot, where n is the number of robot joints, and each element represents the angle vector of each joint relative to its zero position. The joint angular velocity vector is the first derivative vector of the joint angle with respect to time, representing the instantaneous motion rate of each joint. The joint angular acceleration vector is the second derivative vector of the joint angle with respect to time, representing the rate of change of the motion rate of each joint; this data is a key input for calculating inertial forces.

[0053] It should be noted that sampling the real-time operating current of the motor refers to capturing the instantaneous current value flowing through the motor windings at a fixed high frequency through a sampling resistor and analog-to-digital converter circuit integrated within the drive controller, which is connected in series in the three-phase drive circuit of the motor. This current value is proportional to the electromagnetic torque generated by the motor. Furthermore, the joint current vector is an n-dimensional column vector, whose elements represent the real-time effective value of the phase current or torque current component of each joint drive motor (directly reflecting the magnitude of the electromagnetic torque generated by each joint motor to overcome load, friction, gravity, and inertial forces).

[0054] It should be noted that measuring the force and torque in the robot's base coordinate system refers to using a six-dimensional force and torque sensor installed between the robot's fixed base and the mounting platform or the ground to detect and decouple the three orthogonal force components and three orthogonal torque components acting on the origin of the base coordinate system in real time, thereby obtaining the base torque value. This sensor is typically based on strain gauges or optical principles to measure the micro-deformation of the structure caused by external contact forces.

[0055] It should be noted that the base torque value It is a six-dimensional vector, usually represented as ,in , and Represented as base coordinate system axis, shaft and Components of force in the axial direction , and These are the torque components about each axis (all external forces and torques transmitted from the robot body to its base are measured directly and independently, for direct detection of collision contacts originating from the robot end effector or links).

[0056] S2. Calculate the torque observation value of each joint based on the drive motor current vector of each joint.

[0057] In a specific example, the calculation of the torque observation value of each joint based on the drive motor current vector of each joint includes: calculating the torque observation value of each joint using a linear mapping formula based on the drive motor current vector and the torque constant vector of each joint. In conclusion Time of day Torque observations of each joint ,in This is represented by the number corresponding to each joint. , This represents the total number of joints. Indicated as in Time of day The torque constant vector of the motor at each joint Indicated as in Time of day The current vector of the drive motor for each joint.

[0058] It should be noted that linear mapping calculation converts the current signal characterizing the electromagnetic torque of the motor into the theoretical torque value at the joint output end through the inherent physical characteristic parameters of the motor.

[0059] It should be noted that the motor torque constant vector is an n-dimensional column vector, where each element is the torque constant of the corresponding joint motor. The torque constant is the ratio of the output torque produced by the motor per unit current. It is a fundamental parameter determined during the motor design and manufacturing process, representing a fixed proportional relationship between current and torque. The drive motor current vector is the joint current vector read at the current moment.

[0060] It should be noted that the torque observation value of the joint is an n-dimensional column vector, where each element represents the estimated output torque of the corresponding joint at time t obtained through current observation. This vector is a calculated sum of all the actual torque components acting on the joint, including: the inertial torque required to drive the load motion, the torque to overcome the Coriolis force and centrifugal force, the torque to balance the effects of gravity, the torque to overcome internal friction of the transmission system, and the external torque generated by external collisions or contacts.

[0061] In one embodiment, the torque observation value of each joint is calculated based on the drive motor current vector of each joint. Specifically, the following steps are taken: First, the drive motor current vector and the motor torque constant vector of each joint are calculated through a linear mapping to obtain the torque observation value (vector) of each joint. In this embodiment, the motor torque constant vector is a pre-calibrated constant parameter vector stored in the controller. For each joint, its torque constant is determined by the motor's design parameters (such as magnetic flux, number of winding turns, etc.) and is determined through testing during motor manufacturing or system integration. During calculation, the control system reads the joint current vector at the current moment and performs element-wise multiplication of the vectors, i.e., for the first joint... For each joint, the torque observation value of that joint is calculated.

[0062] It should be noted that the torque observation value of the joint reflects the total torque "observed" from the motor end and acting on the joint output shaft. According to the dynamic relationship between the motor and the transmission system, this torque is theoretically equal to the sum of all torques required to drive the load, that is, the sum of the predicted torque calculated by the robot's inverse dynamics model and any unmodeled torques (most importantly, external collision torques).

[0063] Furthermore, the linear mapping calculation formula is physically rigorous, based on the fundamental electromagnetic torque equations of DC motors or permanent magnet synchronous motors. Its calculation process is simple and efficient, requiring no complex model calculations, and can provide torque observations with extremely low latency, laying the foundation for rapid response in real-time collision detection.

[0064] S3. Based on the state data of each joint, calculate the predicted torque value of each joint under collision-free conditions using the robot inverse dynamics model.

[0065] In a specific example, the step of calculating the torque prediction value of each joint under collision-free conditions based on the state data of each joint using the robot inverse dynamics model includes: S401, analyzing and deriving the Coriolis and centrifugal torque vectors based on the joint angle vector and joint angular velocity vector.

[0066] S402, Based on the joint angle vector, obtain the gravitational moment vector corresponding to the current robot configuration from the robot inverse dynamics model database.

[0067] S403, based on the joint angular velocity vector, obtain the friction torque vector corresponding to the current joint motion speed from the robot inverse dynamics model database.

[0068] S404, perform vector addition on the inertial torque vector, the Coriolis and centrifugal torque vector, the gravitational torque vector and the frictional torque vector to obtain the torque prediction value of the joint under the condition of no external collision, and thereby obtain the torque prediction value of each joint.

[0069] It should be noted that obtaining the gravitational torque vector from the robot inverse dynamics model database refers to retrieving the equilibrium torque vector generated at each joint by gravity acting on each link of the robot, based on the current joint angle vector (i.e., robot posture). This vector represents the static torque required to resist gravity and maintain the robot's configuration in the current posture.

[0070] It should be noted that obtaining the friction torque vector from the robot inverse dynamics model database refers to querying the database based on a preset friction model (such as a Coulomb friction plus viscous friction model) to obtain the friction torque vector according to the current joint angular velocity vector. This vector physically represents the resistance torque generated inside each joint transmission system (such as reducer, bearing) due to relative motion, and its magnitude and direction are usually related to the joint velocity.

[0071] It should be noted that performing vector addition involves adding the four vectors with the same physical dimensions (torque) – the inertial torque vector, the Coriolis and centrifugal torque vector, the gravitational torque vector, and the frictional torque vector – element by element.

[0072] It should be noted that the predicted joint torque vector is an n-dimensional vector (n being the number of joints) obtained by adding the aforementioned vectors. This vector fully represents the theoretically required driving torque value applied to each joint by the robot controller to accurately track the current motion trajectory under ideal conditions, without any external disturbances or collisions. Its calculation formula is expressed as follows: ,in This is expressed as the predicted torque value of the joint, and from this, the predicted torque value of each joint is obtained. , Represented as components of inertial torque, Represented as the mass matrix, it is an n×n symmetric positive definite matrix whose elements are joint angle vectors. The matrix represents the equivalent mass, inertia tensor, and coupled inertia of each link in the current robot configuration. This is represented as a joint angular acceleration vector. A matrix multiplication operation with the mass matrix yields a vector with dimensions of torque. This term represents the torque required to generate the current angular acceleration of each joint. It is represented as the Coriolis force and centrifugal torque components. Represented as the Coriolis and centripetal moment matrix, it is an n×n matrix whose elements are also joint angles. and joint angular velocity The function contains coefficients for forces (centrifugal force) that are related to the square of velocity due to the motion of the linkage, and forces (Coriolis force) that are cross-related to velocity and acceleration. This is represented as a joint angular velocity vector. The entire term... This ensures that the dimensions of the result are consistent with those of torque. It represents the torque required to overcome the dynamic coupling effect caused by velocity in the robot's links during movement. Represented as a gravitational torque vector, it is an n-dimensional vector whose elements are joint angle vectors. This is a function of the vector. This vector directly represents the static equilibrium torque generated by gravity on each joint axis under the current robot posture. Its dimension is directly torque. This term is position-dependent and independent of the robot's speed. Represented as a frictional torque vector, it is an n-dimensional vector, typically modeled as joint angular velocity. This is a function of velocity. Common models include viscous friction (proportional to velocity) and Coulomb friction (a constant opposite to velocity). Its dimension is directly torque. This term represents the additional torque required to overcome the frictional resistance inside transmission components such as kinetic motors and reducers.

[0073] Furthermore, the sum represents the theoretically required joint torque to drive the robot to complete the current predetermined motion without external collision forces. This calculation process is mathematically and physically complete, with all components having consistent dimensions (all torques), allowing for vector addition.

[0074] In a specific example, the step of analyzing and deriving the Coriolis and centrifugal torque vectors based on the joint angle vector and joint angular velocity vector includes: S501, obtaining the mass matrix corresponding to the current robot configuration from a pre-built robot inverse dynamics model database based on the joint angle vector.

[0075] S502, based on the joint angular velocity vector, joint angular acceleration vector and mass matrix, perform matrix multiplication to calculate the inertial force components and obtain the inertial torque vector.

[0076] S503, based on the joint angle vector and joint angular velocity vector, obtain the Coriolis and centripetal torque matrices corresponding to the current robot configuration and motion speed from the robot inverse dynamics model database.

[0077] Based on the joint angle vector and joint angular velocity vector, S504 analyzes and obtains the Coriolis force and centrifugal force components, and then obtains the Coriolis and centrifugal torque vectors.

[0078] It should be noted that obtaining the mass matrix from the pre-built robot inverse dynamics model database refers to querying or calculating in real time the mass matrix describing the mass and inertia distribution of each link of the robot, based on the current joint angle vectors of the robot, after offline calibration and storage in the dynamic parameter database of the robot controller. The mass matrix is ​​a function of the joint angles, and its element values ​​change with the robot's posture (characterizing the influence coefficient of joint acceleration on joint torque under the current configuration).

[0079] It should be noted that performing matrix multiplication to calculate the inertial force components involves multiplying the mass matrix representing the inertial properties of each link in the robot with the joint angular acceleration vector describing the changes in acceleration of each joint, thereby obtaining the inertial torque vector. This operation is physically equivalent to calculating the sum of the inertial forces of each link in the robot in joint space. The output inertial torque vector has the dimension of torque, representing the inertial torque that needs to be overcome to drive the robot to generate the current acceleration.

[0080] It should be noted that obtaining the Coriolis and centripetal moment matrices from the robot inverse dynamics model database refers to querying the dynamics database for matrices related to velocity coupling based on the current joint angle vector and joint angular velocity vector. The Coriolis and centripetal moment matrices contain the coefficients of the Coriolis force and centrifugal force generated by the robot's link motion, and their values ​​depend on both the robot's configuration and motion speed.

[0081] It should be noted that performing matrix-vector multiplication to calculate the Coriolis force and centrifugal force components involves multiplying the Coriolis and centripetal torque matrices by the joint angular velocity vector. This operation represents the calculation of the sum of the Coriolis and centrifugal forces generated by the robot's link motion in joint space, and its output Coriolis and centrifugal torque vectors are in the dimension of torque.

[0082] This application utilizes the robot's dynamic model to calculate the theoretically expected torque based on the current motion state (position, velocity, acceleration), providing an ideal "collision-free" reference benchmark. This predicted value can dynamically adapt to load variations under different robot postures and speeds, avoiding false alarms (e.g., misjudged as collisions during high-speed motion) or missed alarms (e.g., inconspicuous collisions during low-speed heavy loads) caused by using fixed thresholds, significantly improving detection accuracy.

[0083] S4. Based on the torque observation values ​​and torque prediction values ​​of each joint, the torque observation value vector of each joint is obtained; and based on the base torque value, the end-collision result is analyzed to generate the collision confirmation mark of each joint.

[0084] In a specific example, the process of deriving the torque observation vector for each joint based on the observed torque values ​​and predicted torque values ​​for each joint includes: calculating the torque vector using a formula. In conclusion Time of day Torque deviation vector of each joint ,in Indicated as in Time of day The vector of torque observations for each joint. Indicated as in Time of day Torque prediction values ​​for each joint.

[0085] It should be noted that the torque deviation vector of the joint is a vector with the same dimension as the number of joints. This torque deviation vector mainly originates from unmodeled dynamic characteristics, parameter errors, and, most importantly, external collision forces. The amplitude or magnitude of each component of this vector is the direct input signal for subsequent collision detection.

[0086] In a specific example, the step of analyzing the end-effector collision results based on the base torque value and generating collision confirmation flags for each joint includes: performing a mapping operation from torque to joint torque based on the base torque value and the robot Jacobian matrix determined by the angle vector of the current joint to obtain the spatial equivalent torque of each joint; and then performing a collision confirmation judgment to generate a collision confirmation flag for each joint.

[0087] It should be noted that the torque-to-joint torque mapping operation is the process of equivalently calculating the base torque value located in the robot base coordinate system to the joint space through the mathematical transformation of the robot kinematics Jacobian matrix.

[0088] Furthermore, the robot Jacobian matrix describes the linear mapping relationship between the linear and angular velocities of the robot's end effector (or any link) and the joint angular velocities under a specific joint configuration. The transpose of the robot Jacobian matrix establishes the dual relationship between the base torque value experienced by the end effector (or a specific point) and the balance torque required by each joint, which is a mechanical mapping relationship derived from the principle of virtual work.

[0089] It should be noted that the joint space equivalent torque is an n-dimensional vector (n is the number of joints), which is calculated using the following formula: In conclusion Time of day Spatial equivalent torque of each joint ,in Indicated as in The base torque value at time [time]. It is represented as a Jacobian matrix, whose elements are dimensionless scaling factors (for the angular velocity mapping part). Its transpose is multiplied by the base torque value, and the resulting product has the same dimensions as the torque.

[0090] It should be noted that the collision confirmation judgment is made by calculating the Euclidean norm (i.e., the magnitude of the vector) of the spatial equivalent torque of each joint and comparing it with a pre-set spatial equivalent torque threshold for each joint.

[0091] It should be noted that the collision confirmation flag is a binary logic variable (represented by 1 and 0), and its generation rule is as follows: when the Euclidean norm of the spatial equivalent torque of a joint is greater than the spatial equivalent torque threshold of that joint, the collision confirmation flag of that joint is 1; otherwise, it is 0. Furthermore, when the base sensor detects and maps the equivalent external force amplitude to the joint space to a value exceeding a small noise threshold, it confirms the existence of a reliable external contact originating from the robot body and transmitted to the base (such as an end-effector collision), and the flag is set to "1" (true); otherwise, it is considered that no reliable external collision force has been detected by the base sensor, and the flag is set to "0" (false).

[0092] This application introduces a base torque value as a global reference system for the first time, cross-validating the collision results of each joint to generate a "collision confirmation flag." This effectively solves the problem of false alarms from single joints. For example, when the robot's end effector collides with an object, the base torque will inevitably change; if a joint shows a collision but the base torque does not respond, it can be determined that the joint itself is faulty or subject to vibration interference. This "global-local" verification mechanism greatly improves the reliability and anti-interference capability of collision event confirmation.

[0093] S5. Based on the historical statistical characteristics of the torque observation vector of each joint and the collision confirmation identifier of each joint, analyze and obtain the time-varying safety confidence function value of each joint, and generate the collision detection threshold of each joint.

[0094] In a specific example, the analysis yields the time-varying safety confidence function value of each joint and generates the collision detection threshold for each joint, including: S801, calculating the statistical characteristics characterizing the degree of abnormality based on the historical data of the joint torque deviation vector within a preset time window.

[0095] S802, based on the statistical characteristics and the collision confirmation flag, the safety confidence function value of each joint at the current moment is calculated using a time-varying safety confidence function.

[0096] S803, based on the safety confidence function value and the preset default static collision detection threshold, the time-varying collision detection threshold of each joint is dynamically generated through proportional scaling calculation.

[0097] It should be noted that the historical data based on the joint torque deviation vector within a preset time window refers to extracting all joint torque deviation vector sequences stored within a fixed time period (e.g., the most recent N control cycles) preceding the current time t. This historical window data records the dynamic changes in the deviation between the observed and predicted values ​​of each joint torque in recent times, and is an important basis for assessing whether the current system is in a normal, suspected interference, or confirmed collision state.

[0098] It should be noted that calculating the statistical properties characterizing the degree of anomaly refers to processing the joint torque deviation vector in the historical window data to quantify its overall level of deviation from zero (i.e., the ideal state without deviation). A typical statistical property is to calculate the root mean square value of the Euclidean norm (i.e., vector magnitude) of the torque deviation vector at each time point within the window. The calculation process is as follows: First, for each historical time point k within the window, calculate the Euclidean norm of the joint torque deviation vector at that time; second, calculate the root mean square value of these N norm values, which serves as a statistic characterizing the overall degree of anomaly in torque deviation within the historical window. This statistic is a scalar; the larger its value, the higher the overall inconsistency between the observed joint torque values ​​and the model predictions in recent history, and the greater the likelihood that the system has encountered unmodeled disturbances or potential collisions.

[0099] It should be noted that the time-varying safety confidence function is a scalar function whose value dynamically reflects the system's confidence in the proposition that "the current state is safe and collision-free." A time-varying safety confidence function of 1 indicates complete confidence in safety, while a lower value indicates less confidence in safety, and the system is more inclined to believe that a collision risk may exist. The calculation of this function aims to fuse the statistical characteristics of historical torque deviations with direct collision confirmation signals from the base sensors for decision-making.

[0100] It should be noted that, based on the aforementioned statistical characteristics and the collision confirmation flag, the safety confidence function value is derived through the following mathematical relationship:

[0101] , and when season ;in Represented as the safety confidence function value, This represents the safety confidence function value from the previous moment, and serves as the initial reference for calculation at this moment. It is represented as a very small positive threshold close to 0, used to determine whether the system is in a "non-anomaly" calm state; Represented as the confidence recovery coefficient, it is a preset small positive number. When the system is judged to be in a calm state, it is used to slowly restore the safety confidence function value to a completely confident safe state (the safety confidence function value equals 1). Take 1 and The smaller of the two values ​​ensures the safety confidence function value. Less than or equal to 1.

[0102] Furthermore, for The confidence decay coefficient at time t is calculated using the following formula: ;in Represented as The confidence decay coefficient at time t is a dimensionless number between 0 and 1, which determines... Compared to The rate of decay; and the larger the confidence decay coefficient value, the faster the decay. This represents the base attenuation weighting coefficient, a preset normal value. It is used to adjust the degree of influence of historical torque deviation statistics on confidence attenuation. It is represented as the root mean square statistic of the torque deviation vector within the historical window, with the dimension of torque, which quantifies the average intensity of recent unmodeled torque disturbances; Represented as the emergency decay weighting coefficient, it is much larger than The preset normal value. Used to provide a strong impact with reduced confidence when the base sensor confirms a collision; This represents the collision confirmation flag, which is a binary variable (0 or 1). When its value is 1, it indicates that the base sensor has clearly detected an external collision force originating from the robot body; when it is 0, it indicates that it has not been confirmed.

[0103] It should be noted that the design of this function implements the behavioral logic described in step S5: when the collision confirmation flag is equal to 1, due to the large value of the emergency decay weight coefficient, Confidence decay coefficient at time A significant increase leads to a decrease in the safety confidence function value. It rapidly decays to a lower value, quickly responding to confirmed collision events; when the root mean square statistic of the torque deviation vector remains high within the historical window but the collision confirmation flag is equal to 0, it mainly relies on... Xiang Shi Confidence decay coefficient at time Maintained at a moderate level, the safety confidence function value It exhibits a slow decay, remaining vigilant for persistent unconfirmed anomalies; when there are no anomalies (the root mean square statistic of the torque deviation vector within the historical window is small and the collision confirmation flag is equal to 0), Confidence decay coefficient at time Extremely small values ​​trigger the recovery mechanism, causing the safety confidence function value to... The gradual recovery to 1 indicates that the system has regained a high level of security confidence.

[0104] It should be noted that the scaling calculation is based on the safety confidence function value and the preset default static collision detection threshold, and its mathematical expression is as follows: ,in exist The first moment The dynamic collision detection threshold for each joint, with the dimension of torque; This represents the preset default static collision detection threshold, which is a positive constant value measured in torque. It is the baseline collision sensitivity threshold used when the system is completely safe and reliable.

[0105] It should be noted that the time-varying collision detection thresholds for each joint are dynamically generated. The process involves using the safety confidence function as an adjustment factor, along with a fixed default threshold. Multiplication. The safety confidence function value is less than or equal to 1, thus generating a dynamic threshold. Less than or equal to .

[0106] It should be noted that the purpose of this dynamic threshold generation mechanism is as follows: when the system's safety confidence is high, the dynamic threshold is close to or equal to the default threshold, and the system maintains normal collision detection sensitivity; when the system's safety confidence decreases due to the detection of continuous anomalies or confirmed collisions, the dynamic threshold will decrease accordingly.

[0107] This application utilizes dynamic threshold generation and time-varying confidence levels. It abandons fixed empirical thresholds and generates dynamic thresholds and safety confidence functions that change with operating conditions by analyzing historical statistical characteristics and combining them with collision confirmation markers. This achieves adaptive threshold adjustment. When robot wear, load changes, or ambient temperature affects current characteristics, the threshold can drift and adjust in real time according to historical statistical characteristics, always maintaining optimal sensitivity. The safety confidence function provides a probabilistic reference for subsequent decisions, avoiding rigid, one-size-fits-all judgments.

[0108] S6. Based on the collision confirmation flags of each joint and the dynamically generated collision detection thresholds of each joint, determine whether a collision has occurred.

[0109] In a specific example, the step of determining whether a collision has occurred based on the collision confirmation identifier of each joint and the dynamically generated collision detection threshold of each joint includes: S901, obtaining the input data required for making a collision decision based on the torque deviation vector of each joint and the dynamic collision detection threshold of each joint.

[0110] S902, based on the torque deviation components of each joint and the corresponding time-varying collision detection threshold, performs an absolute value comparison operation to generate a Boolean logic flag characterizing whether the joint has abnormally exceeded the limit.

[0111] S903 performs a logical "OR" operation based on the Boolean logic flags of all joints to determine the global collision state, and generates and outputs a collision alarm signal and collision joint position identifier based on the determination result.

[0112] It should be noted that the "absolute value comparison operation" performed in step S902 refers to calculating the absolute value of the torque deviation component of the i-th joint and comparing it with the time-varying collision detection threshold of that joint. This operation aims to quantify whether the amplitude of the current joint torque deviation exceeds the safety boundary allowed by the system under the current safety confidence level.

[0113] Furthermore, the specific process of "generating a Boolean logic flag to indicate whether the joint has exceeded the limit abnormally" is as follows: if the absolute value of the joint torque deviation is greater than the time-varying collision detection threshold of the joint, it is determined that the torque abnormality of the joint may be caused by a collision, and a logic "true" flag (represented by 1) is generated; otherwise, it is determined that the current torque deviation of the joint is within the acceptable dynamic safety range, and a logic "false" flag (represented by 0) is generated.

[0114] It should be noted that the Boolean logic flag is a binary variable (a preliminary judgment result on whether a single joint has exceeded the collision level torque limit). This comparison is performed in parallel for all joints, resulting in a flag vector with the same dimension as the number of joints.

[0115] It should be noted that the "logical OR operation" performed in step S903 refers to checking whether at least one of the Boolean logic flags of all joints is logically "true" (1). Mathematically, this operation is represented as performing a logical OR aggregation on the elements of the flag vector.

[0116] It should be noted that the "determining the global collision state" is based on the result of a logical "OR" operation: if the result is "true", the robot as a whole is determined to have experienced a collision; if the result is "false", all joints of the robot are determined to be in a safe state. This judgment logic ensures that any abnormality in any joint can trigger a global collision response.

[0117] It should be noted that the specific process of "generating and outputting collision alarm signals and collision joint position identifiers" is as follows: when a global collision is determined to occur, a high-priority digital or level signal is immediately generated as a collision alarm signal to trigger safety protocols such as emergency stop or deceleration of the robot; at the same time, all joint index numbers or position information with Boolean logic flags set to "true" are packaged and output as collision joint position identifiers to accurately locate the affected joints.

[0118] Please see Figure 2 As shown, this application provides a system for a collision detection method for coordinated monitoring of robot joint current and torque in a second aspect.

[0119] The system 100 of the robot joint current and torque collaborative monitoring collision detection method of the present invention can be installed in an electronic device. Depending on the functions implemented, the system 100 may include a data acquisition module 101, a torque observation value calculation module 102, a torque prediction value calculation module 103, a collision confirmation identifier generation module 104, a collision detection threshold generation module 105, and a collision determination module 106. The module described in this invention can also be called a unit, which refers to a series of computer program segments that can be executed by the processor of an electronic device and can perform a fixed function, and is stored in the memory of the electronic device.

[0120] In this embodiment, the functions of each module / unit are as follows: The data acquisition module is used to acquire the status data of each joint of the robot, the current vector of the drive motor of each joint, and the torque value of the base in real time.

[0121] The torque observation calculation module calculates the torque observation value of each joint based on the drive motor current vector of each joint.

[0122] The torque prediction calculation module calculates the torque prediction value of each joint under collision-free conditions based on the state data of each joint and through the robot inverse dynamics model.

[0123] The collision confirmation marker generation module derives the torque observation value vector for each joint based on the torque observation value and the torque prediction value of each joint; and analyzes the end-collision result based on the base torque value to generate a collision confirmation marker for each joint.

[0124] The collision detection threshold generation module analyzes and derives the time-varying safety confidence function value of each joint based on the historical statistical characteristics of the torque observation vector of each joint and the collision confirmation identifier of each joint, and generates the collision detection threshold of each joint.

[0125] The collision determination module determines whether a collision has occurred based on the collision confirmation flags of each joint and the dynamically generated collision detection thresholds for each joint.

[0126] The robot joint current and torque collaborative monitoring collision detection method and system provided in this application can accurately distinguish between joint body disturbances and actual end-effector collisions by integrating the joint states, motor current observations, and base torque values, effectively filtering out interference caused by load changes or high-speed acceleration and deceleration. Secondly, by introducing a time-varying safety confidence function based on the historical statistical characteristics of torque observations, the collision detection threshold can be adaptively adjusted according to the robot's real-time operating conditions, solving the problem that fixed thresholds are prone to missed detections under high-speed heavy loads and false detections under low-speed light loads. Finally, by comparing the dynamic threshold with the real-time torque deviation joint by joint, the collision joint can be accurately located and an alarm can be issued at the moment of collision, gaining valuable time for the robot's rapid and safe response and greatly improving the safety of human-robot collaboration.

[0127] In the several embodiments provided by this invention, it should be understood that the disclosed methods and systems can be implemented in other ways. For example, the system embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and other division methods may be used in actual implementation.

[0128] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0129] Furthermore, the functional modules in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or in the form of hardware plus software functional modules.

[0130] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention.

[0131] The embodiments of this application can acquire and process relevant data based on artificial intelligence technology. Artificial intelligence is the theory, method, technology, and application system that uses digital computers or machines controlled by digital computers to simulate, extend, and expand human intelligence, perceive the environment, acquire knowledge, and use that knowledge to obtain optimal results.

[0132] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A collision detection method for coordinated monitoring of robot joint current and torque, characterized in that, include: S1. Real-time acquisition of the status data of each joint of the robot, the current vector of the drive motor of each joint, and the torque value of the base; S2. Calculate the torque observation value of each joint based on the drive motor current vector of each joint; S3. Based on the state data of each joint, calculate the predicted torque value of each joint under collision-free conditions using the robot inverse dynamics model; S4. Based on the torque observation values ​​and torque prediction values ​​of each joint, the torque observation value vector of each joint is obtained. Based on the base torque value, the end-collision results are analyzed, and collision confirmation marks for each joint are generated; S5. Based on the historical statistical characteristics of the torque observation vector of each joint and the collision confirmation identifier of each joint, analyze and obtain the time-varying safety confidence function value of each joint, and generate the collision detection threshold of each joint. S6. Based on the collision confirmation flags of each joint and the dynamically generated collision detection thresholds of each joint, determine whether a collision has occurred.

2. The robot joint current and torque coordinated monitoring and collision detection method according to claim 1, characterized in that, The joint state data includes the robot's joint angle vector, joint angular velocity vector, and joint angular acceleration vector.

3. The robot joint current and torque coordinated monitoring and collision detection method according to claim 1, characterized in that, The calculation of torque observation values ​​for each joint based on the drive motor current vector of each joint includes: Based on the drive motor current vector and the motor torque constant vector of each joint, a linear mapping formula is used for calculation. In conclusion Time of day Torque observations of each joint ,in This is represented by the number corresponding to each joint. , This represents the total number of joints. Indicated as in Time of day The torque constant vector of the motor at each joint Indicated as in Time of day The current vector of the drive motor for each joint.

4. The robot joint current and torque coordinated monitoring and collision detection method according to claim 1, characterized in that, The calculation of the predicted torque values ​​of each joint under collision-free conditions using a robot inverse dynamics model, based on the state data of each joint, includes: S401, based on the joint angle vector and joint angular velocity vector, the Coriolis and centrifugal torque vectors are analyzed and obtained; S402, Based on the joint angle vector, obtain the gravitational moment vector corresponding to the current robot configuration from the robot inverse dynamics model database; S403, Based on the joint angular velocity vector, obtain the friction torque vector corresponding to the current joint movement speed from the robot inverse dynamics model database; S404, perform vector addition on the inertial torque vector, the Coriolis and centrifugal torque vector, the gravitational torque vector and the frictional torque vector to obtain the torque prediction value of the joint under the condition of no external collision, and thereby obtain the torque prediction value of each joint.

5. The robot joint current and torque coordinated monitoring and collision detection method according to claim 4, characterized in that, The Coriolis force and centrifugal torque vectors are derived from the analysis based on the joint angle vector and joint angular velocity vector, including: S501, Based on the joint angle vector, obtain the mass matrix corresponding to the current robot configuration from the pre-built robot inverse dynamics model database; S502, based on the joint angular velocity vector, joint angular acceleration vector and mass matrix, perform matrix multiplication to calculate the inertial force components and obtain the inertial torque vector; S503, based on the joint angle vector and joint angular velocity vector, obtain the Coriolis and centripetal torque matrices corresponding to the current robot configuration and motion speed from the robot inverse dynamics model database; Based on the joint angle vector and joint angular velocity vector, S504 analyzes and obtains the Coriolis force and centrifugal force components, and then obtains the Coriolis and centrifugal torque vectors.

6. The robot joint current and torque coordinated monitoring and collision detection method according to claim 1, characterized in that, The process of deriving a torque observation vector for each joint based on the observed torque values ​​and predicted torque values ​​of each joint includes: Through calculation formula In conclusion Time of day Torque deviation vector of each joint ,in Indicated as in Time of day The vector of torque observations for each joint. Indicated as in Time of day Torque prediction values ​​for each joint.

7. The robot joint current and torque coordinated monitoring and collision detection method according to claim 1, characterized in that, The process of analyzing the end-collision results based on the base torque value and generating collision confirmation markers for each joint includes: Based on the base torque value and the robot Jacobian matrix determined by the angle vector of the current joint, a mapping operation is performed from torque to joint torque to obtain the spatial equivalent torque of each joint; then a collision confirmation judgment is performed to generate a collision confirmation flag for each joint.

8. The robot joint current and torque coordinated monitoring and collision detection method according to claim 1, characterized in that, The analysis yields time-varying safety confidence function values ​​for each joint and generates collision detection thresholds for each joint, including: S801, Based on the historical data of the joint torque deviation vector within a preset time window, calculate the statistical characteristics characterizing its degree of abnormality; S802, based on the statistical characteristics and the collision confirmation flag, the safety confidence function value of each joint at the current moment is calculated using the time-varying safety confidence function; S803, based on the safety confidence function value and the preset default static collision detection threshold, the time-varying collision detection threshold of each joint is dynamically generated through proportional scaling calculation.

9. The robot joint current and torque coordinated monitoring and collision detection method according to claim 1, characterized in that, The method of determining whether a collision has occurred based on the collision confirmation markers of each joint and the dynamically generated collision detection thresholds for each joint includes: S901, based on the torque deviation vector of each joint and the dynamic collision detection threshold of each joint, obtain the input data required for collision decision-making; S902, based on the torque deviation components of each joint and the corresponding time-varying collision detection threshold, performs an absolute value comparison operation to generate a Boolean logic flag characterizing whether the joint has abnormally exceeded the limit. S903 performs a logical "OR" operation based on the Boolean logic flags of all joints to determine the global collision state, and generates and outputs a collision alarm signal and collision joint position identifier based on the determination result.

10. A system for implementing the robot joint current and torque coordinated monitoring and collision detection method according to any one of claims 1-9, characterized in that, include: The data acquisition module is used to acquire the status data of each joint of the robot, the current vector of the drive motor of each joint, and the torque value of the base in real time. The torque observation calculation module calculates the torque observation value of each joint based on the drive motor current vector of each joint. The torque prediction calculation module calculates the torque prediction value of each joint under collision-free conditions based on the state data of each joint and through the robot inverse dynamics model. The collision confirmation identifier generation module generates a torque observation value vector for each joint based on the torque observation value and the torque prediction value of each joint. Based on the base torque value, the end-collision results are analyzed, and collision confirmation marks for each joint are generated; The collision detection threshold generation module analyzes and derives the time-varying safety confidence function value of each joint based on the historical statistical characteristics of the torque observation vector of each joint and the collision confirmation identifier of each joint, and generates the collision detection threshold of each joint. The collision determination module determines whether a collision has occurred based on the collision confirmation flags of each joint and the dynamically generated collision detection thresholds for each joint.

Images