Mechanical part collision detection method based on multi-information fusion

Abstract

The invention relates to the technical field of robots, in particular to a mechanical part collision detection method based on multi-information fusion, and the method comprises the steps: obtaining the joint angle and angular velocity of a robot moving part; estimating the tail end force of the series motion part by using a robot kinetic model; total energy is calculated through kinetic energy and potential energy of the moving part, and the energy change rate is calculated according to the total energy; and constructing a comprehensive characteristic index by using the energy change rate, the tail end contact force and the angular velocity of the moving part. The problems that in the prior art, the detection precision of a single sensor is interfered by the environment, the cost is high due to dependence on an expensive force sensor, and the collision feature recognition capacity is weak under the complex working condition due to insufficient multi-source data fusion depth are solved.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a collision detection method for mechanical parts based on multi-information fusion. Background Technology

[0002] Robots have been widely used in industrial manufacturing, warehousing and logistics, surgical medical care, security inspection, agricultural harvesting and household services.

[0003] Robot collision detection technology aims to ensure real-time safety and equipment integrity in human-robot collaboration. Collision detection relies on information processing from single or limited sensors, and the core challenge lies in balancing detection accuracy, cost, and environmental adaptability. Current mainstream solutions include: 1. The motion parameters are obtained by the joint encoder, the theoretical torque is estimated by combining the dynamic model, and the collision is judged by comparing with the actual motor signal. This method relies on accurate dynamic parameter calibration and is sensitive to interference from flexible joints in low-speed contact or complex environments, which can easily lead to misjudgment or missed judgment. 2. Installing force sensors on joints or ends to directly measure contact force / torque has high detection accuracy, but the sensors are more expensive, the structure is more complex, and the signal is easily affected by mechanical vibration or environmental noise, requiring additional filtering. 3. Collisions can be identified by monitoring abnormal changes in motor power, current and other signals. This method is suitable for simple working conditions, but under high load or complex motion scenarios, the signal characteristics are easily confused with normal operating fluctuations, and the ability to identify the direction and type of collision is limited. Summary of the Invention

[0004] To address the shortcomings of existing methods, this invention, without relying on force sensors, constructs a detection system that accurately identifies collisions or foot contact states of robotic arms or legs by fusing multi-source information such as joint motion parameters collected by encoders, end-effector forces estimated based on dynamic models, and joint energy changes. This solves the problems in existing technologies, such as the accuracy of single-sensor detection being affected by environmental interference, high costs due to reliance on expensive force sensors, and weak collision feature recognition capabilities under complex working conditions due to insufficient depth of multi-source data fusion.

[0005] The technical solution adopted in this invention is: a collision detection method for mechanical parts based on multi-information fusion, comprising the following steps: Step 1: Obtain the joint angles and angular velocities of the robot's moving parts; In a preferred embodiment of the present invention, outlier removal and noise filtering are performed on the joint angle and angular velocity data.

[0006] Step 2: Estimate the end force of the series-moving components using the robot dynamics model; In a preferred embodiment of the present invention, step two specifically includes: Calculate joint torque , The torque constant of the motor; This refers to the actual input current of the joint motor; Efficiency of the joint transmission mechanism; This refers to the joint transmission ratio; Joint friction torque; Using the Newton-Euler equations to measure joint torques Inverse solution, the end force of the moving part is obtained. , Jacobi for moving parts; The inertia matrix; The matrix represents the Coriolis force and the centrifugal force. This is the gravity vector.

[0007] Step 3: Calculate the total energy using the kinetic and potential energy of the moving parts, and calculate the rate of change of energy based on the total energy; In a preferred embodiment of the present invention, the formula for the rate of energy change is: , , ,

[0008] in, m The mass of the connecting rod; l It is the length of the connecting rod. For joint angle, Angular velocity, t For the current moment, For time intervals.

[0009] Step 4: Construct comprehensive characteristic indicators using the energy change rate, end contact force, and angular velocity of the moving parts; In a preferred embodiment of the present invention, the formula for the comprehensive characteristic index is as follows:

[0010] in, , and These are the weighting coefficients.

[0011] In a preferred embodiment of the present invention, the formula for the weighting coefficient is:

[0012] in, For scene type coefficients; a 1. a 2. a 3 is the correction factor; This represents the cumulative error from the most recent c iterations.

[0013] As a preferred embodiment of the present invention, it further includes: comparing the comprehensive feature index with a preset threshold to determine whether there is a collision and / or contact.

[0014] As a preferred embodiment of the present invention , .

[0015] The beneficial effects of this invention are: 1. Estimate foot contact force by encoder data and dynamic model, and construct multi-dimensional detection index by combining energy change characteristics (such as kinetic energy and power fluctuation) to avoid hardware dependence on external force sensors; 2. Deep feature-level fusion of multi-dimensional information such as kinematic parameters (angular velocity, joint angle), force estimation, and energy change rate is performed to form a more comprehensive collision discrimination feature space; 3. Construct an adaptive decision-making mechanism to dynamically adjust the detection threshold based on historical data, adapt to different robot structures and operating scenarios, and improve the detection reliability in complex environments. Attached Figure Description

[0016] Figure 1 This is a flowchart of the mechanical component collision detection method based on multi-information fusion of the present invention; Figure 2 This is a schematic diagram of the working of the robot's moving parts. Detailed Implementation

[0017] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0018] like Figure 1 As shown, a collision detection method for mechanical parts based on multi-information fusion includes the following steps: Step 1: Obtain the joint angles and angular velocities of the robot's moving parts; The moving parts are mainly robotic arms and / or robotic legs, which, based on their characteristics, are collectively referred to as serial moving parts below; like Figure 2 The encoder is installed at each joint of the moving parts to provide high-precision angles. and angular velocity The measurements provide fundamental data for subsequent end force estimation and energy calculation; For each key joint of a serially moving component (such as the first three high-power joints in collaborative robots, and the hip and knee joints in legged robots), data are collected synchronously with the robot's gait / control cycle. Encoder acquires joint angles angular velocity It is used for forward kinematics, end force estimation and joint angle change feature extraction. The acquisition frequency needs to match the motion speed of the moving parts, usually not less than 500Hz, to avoid data lag causing detection delay. Motor current data: The phase current of the joint motor is collected and obtained through the current sampling module of the motor drive circuit. It is used to calculate the joint output torque, assist in the end force estimation of the dynamic model, and is synchronized with the encoder data acquisition frequency. In particular, considering the multi-leg coordination when a legged robot walks, it is necessary to perform "gait phase labeling" on the data of a single leg; combined with the robot's gait planning instructions (such as "leg lift phase" and "foot landing phase"), detection is only started in the "foot landing phase" to avoid misjudgment in the leg lift phase; If data from one leg is missing (e.g., due to a temporary encoder malfunction), historical data from the same leg will be used temporarily to ensure uninterrupted detection.

[0019] Outlier removal: If the encoder angle / angular velocity or motor current at a certain moment exceeds the normal operating range (e.g., exceeds the mean ± 3 times the standard deviation), it is judged as abnormal data, and the mean of the data at the previous moment and the next moment is used to replace it.

[0020] Noise filtering: For high-frequency noise (such as workshop power grid interference) in the motor current signal, low-pass filtering (such as Butterworth filtering) is used to process it while preserving the effective torque characteristics in the signal.

[0021] Step 2: Use the robot dynamics model to estimate the end force (in contact with the environment) of the serially moving parts. Based on the robot's linkage structure (such as base, upper arm, forearm, and end effector), core dynamic parameters are obtained through software drawing extraction and offline experimental methods, specifically including: Initialize the dynamic model parameter calibration: Inertial parameters of each link: mass, length, and inertia matrix; the inertia matrix is ​​measured by a weighing instrument or inertial measurement equipment, or calculated based on the material density and structural dimensions of the robotic arm design drawings; Joint motion parameters: Joint transmission ratio and friction torque were obtained by fitting the relationship curve between friction torque and angular velocity through experiments of uniform joint motion under no-load / load conditions. Output joint torque for: (1) in, Motor torque constant (unit: N) m / A), determined by the motor model, is the inherent proportionality coefficient between motor current and output torque; The actual input current of the joint motor (unit: A) is collected in real time by the current sampling module of the motor drive circuit; The efficiency of the joint transmission mechanism (unitless, range 0 < η ≤ 1) needs to take into account gear meshing losses, bearing friction, etc., and is determined by the transmission structure (such as harmonic reducer, RV reducer); This is the joint transmission ratio (unitless). Joint friction torque (unit: N) The value of m is related to the friction coefficient, joint angular velocity, and normal force, and needs to be calculated through experimental fitting.

[0022] Using the DH parameter method, the spatial position of the foot, especially the height in the Z direction, is calculated for the hip and knee joint angles of a single leg to determine the distance between the foot and the ground, providing a positional reference for "foot contact". Considering that the foot mainly bears the force perpendicular to the ground (Z direction) when landing, the Newton-Euler equation is simplified; the force components in the horizontal direction (X / Y) are ignored (unless the ground is uneven and causes the risk of rollover, in which case they can be retained), and only the end force in the Z direction is calculated; combined with the torques of the hip and knee joints (calculated by current), they are substituted into the simplified dynamic equation (Equation 2); in the equation, the force F is the force and torque in three directions included in the six-dimensional information (Fx, Fy, Fz, rx, ry, rz), and Fz is taken as the foot end force in the Z direction as the end force characteristic, which directly reflects the grounding strength; The Newton-Euler equations are used to estimate the end force, and the formula is as follows: (2) in, Joint torque; Jacobi for moving parts; For end force; The inertia matrix represents the joint angles. The function reflects the inertial characteristics of various parts of the robot and demonstrates the influence of the robot's mass distribution on its motion under different postures; The matrix of Coriolis force and centrifugal force, and and Related, describing the Coriolis force and centrifugal force generated by the relative motion between the joints of a robot during its movement; The gravity vector depends on This represents the effect of gravity on the robot's joints; angular acceleration. Yes Obtained by difference (e.g., first-order backward difference); That is, joint torque = inertial torque (inertia matrix × joint angular acceleration) + Coriolis force / centrifugal force (Coriolis matrix × joint angular velocity) + gravitational torque (gravity vector, related to joint angle) + joint torque corresponding to end force (Jacobi matrix transpose × end force).

[0023] terminal force F Solution: Substituting the known joint torques, inertia matrix, Coriolis matrix, gravity vector, and Jacobian matrix into the equation, the end-effector force vector is solved inversely. The magnitude of this vector is taken as the "end-effector force characteristic" (directly reflecting the contact strength between the end-effector and the external environment), i.e.: ; Step 3: Calculate the kinetic and potential energy of the moving parts to obtain the total energy; derive the rate of change of energy from the total energy. The Lagrange method (applicable to energy calculation of multi-rigid-body systems) is used to calculate the total energy and rate of change of energy of joints in series-moving components. The steps are as follows: Kinetic energy calculation: For each joint, the rotational kinetic energy of the individual joint is calculated based on its inertial parameters (inertia matrix) and angular velocity. The total kinetic energy of the series moving parts is obtained by summing them up (the magnitude of kinetic energy reflects the activity level of the joint movement, and the kinetic energy will change abruptly during a collision). The formula for calculating kinetic energy is as follows: (3) in, m For the mass of the connecting rod, l It is the length of the link; this formula shows that the kinetic energy of the joint is proportional to the square of the link's mass and angular velocity, reflecting the degree of joint mobility.

[0024] Potential energy calculation: For each link, calculate the gravitational potential energy of the link in the gravitational field based on its mass, center of gravity position (determined by the link length and structure) and joint angle, and sum them to obtain the total potential energy of the robotic arm (the change in potential energy reflects the change in the spatial position of the link, and the rate of change of potential energy will be abnormal during collision). Formula for calculating potential energy: (4) in, g It is gravitational acceleration; the magnitude of potential energy depends on This reflects the positional changes of the joints in the gravitational field.

[0025] Total Energy Formula: (5) Energy change rate calculation: Take the total energy difference between the current moment and the previous moment, divide it by the time interval (acquisition cycle) to obtain the energy change rate, which is used as the "energy change characteristic" (energy transfer or loss during collision causes the absolute value of the change rate to increase significantly).

[0026] Formula for calculating the rate of change of energy: (6) in, It is the current moment. t Total energy; It is the total energy at the previous moment. It is a time interval; the energy change rate reflects how the joint energy changes over time. When a collision or landing on the ground occurs, the energy change rate will show a significant abrupt change.

[0027] During landing, the leg transitions from "acceleration during the downward swing" to "deceleration upon landing," resulting in a significant change in energy. Kinetic energy: Before landing, the kinetic energy increases as the leg swings down; after contact with the ground, the kinetic energy decreases rapidly (partially converted into potential energy or transferred to the ground), so the absolute value of the rate of change of kinetic energy will increase significantly. Potential energy: Before landing, the height of the foot decreases, and the potential energy decreases; after contact with the ground, the height remains unchanged, and the potential energy tends to stabilize. Therefore, the rate of change of potential energy changes from negative to close to 0. Energy change rate: The absolute value of the total energy change rate is taken as the "energy change characteristic". That is, the energy change rate value will reach a peak at the moment of contact.

[0028] Step 4: Construct comprehensive characteristic indicators using the energy change rate, end contact force, and angular velocity of the moving parts; The formula for the comprehensive characteristic index is: (7) in, , and These are the weighting coefficients, and ; The weighting coefficients can be adjusted according to the actual application scenario and the specific characteristics of the robot to balance the role of end force and energy change in contact detection.

[0029] Real-time assessment: Combining comprehensive feature indicators S With preset threshold S th In comparison, if S > S th If a collision occurs, the robotic arm is identified as having "occurred," triggering an emergency deceleration or stop command; if S ≤ S th If the data is collected, it is considered "normal movement" and the data collection cycle continues. Adaptive adjustment: If 2-3 false alarms occur consecutively (e.g., an alarm is triggered when there is no collision, or an alarm is triggered when there is a collision), the weights and thresholds will be dynamically adjusted based on historical data (feature distribution of the last 100 normal / collision scenarios). If a collision is missed (not triggered): appropriately lower the threshold or increase the weight of the end force feature. ; If a misjudgment occurs (no collision triggered): appropriately increase the threshold or decrease the weight of the energy change feature. .

[0030] Construct an adaptive weight update function;

[0031] Table 1 Adaptive Weight Coefficient Table (8) Read the preset value every 20ms. Update after each judgment and ,calculate (Only the most recent 3 data points are retained) to enable dynamic real-time adjustment of the weighting coefficients.

[0032] Step 5: Compare the comprehensive feature index value with the preset threshold to determine whether there is a collision and / or contact; Set a comprehensive threshold When the comprehensive characteristic index S exceeds this threshold, it is determined that a collision has occurred or the foot of the legged robot has come into contact with the ground, that is: (9) (10) Preset threshold setting: determined through pre-experimentation (e.g., continuously perform 20-50 "non-collision normal motion" and "simulated collision motion", statistically analyze the distribution range of comprehensive feature indicators under the two scenarios, and take the critical value of the two ranges as the preset threshold).

[0033] To enable the detection algorithm to adapt to different working environments and robot load variations, an adaptive adjustment mechanism is introduced; the weight coefficients are dynamically adjusted through analysis and learning of historical detection data. , , and comprehensive threshold For example, when a robot walks on surfaces with different coefficients of friction, the weights and thresholds are automatically optimized based on energy changes and the actual performance of the end effector force to improve the accuracy and reliability of detection. Through the above technical solutions, the complementarity of encoder data, end effector force estimation, and energy change information is fully utilized to achieve effective detection of robotic arm (robotic leg) collisions / foot contact of legged robots without force sensors.

[0034] This invention was experimentally tested on collaborative robots (for collision detection of robotic arms) and quadruped robots (for foot contact detection). Compared with traditional collision detection methods, the core performance improvements are as follows: In terms of response time, the average response time of this invention is 10ms (control cycle is 1ms), which is about 50% shorter than the traditional method (20ms), and can quickly trigger deceleration or gait adjustment commands to meet real-time control requirements; In terms of detection accuracy, in the hard-surface assembly scenario of collaborative robots, the false positive rate of this invention is ≤2% and the false negative rate is ≤5%, with significant optimization of overall accuracy; In terms of environmental adaptability, facing sudden load changes of collaborative robots (1kg load superimposed on the end effector) and different ground switching scenarios of quadruped robots (concrete / grass), this invention still maintains a detection accuracy of over 90% through weight adaptive adjustment, and its environmental adaptability is significantly better.

[0035] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

Claims

1. A collision detection method for mechanical parts based on multi-information fusion, characterized in that, Includes the following steps: Step 1: Obtain the joint angles and angular velocities of the robot's moving parts; Step 2: Estimate the end force of the series-moving components using the robot dynamics model; Step 3: Calculate the total energy using the kinetic and potential energy of the moving parts, and calculate the rate of change of energy based on the total energy; Step 4: Construct a comprehensive characteristic index using the energy change rate, end contact force, and angular velocity of the moving parts.

2. The mechanical component collision detection method based on multi-information fusion according to claim 1, characterized in that, Step two specifically includes: Calculate joint torque , The torque constant of the motor; This refers to the actual input current of the joint motor; Efficiency of the joint transmission mechanism; This refers to the joint transmission ratio; Joint friction torque; Using the Newton-Euler equations to measure joint torques Inverse solution, the end force of the moving part is obtained. , Jacobi for moving parts; The inertia matrix; The matrix represents the Coriolis force and the centrifugal force. This is the gravity vector.

3. The mechanical component collision detection method based on multi-information fusion according to claim 1, characterized in that, The formula for the rate of change of energy is: ， ， ， in, m The mass of the connecting rod; l It is the length of the connecting rod. For joint angle, Angular velocity, t For the current moment, For time intervals.

4. The mechanical component collision detection method based on multi-information fusion according to any one of claims 2 or 3, characterized in that, The formula for the comprehensive characteristic index is: in, , and These are the weighting coefficients.

5. The mechanical component collision detection method based on multi-information fusion according to claim 4, characterized in that, The formula for the weighting coefficient is: in, For scene type coefficients; a 1. a 2. a 3 is the correction factor; For the most recent c Accumulated error over time.

6. The mechanical component collision detection method based on multi-information fusion according to claim 4, characterized in that, Also includes: The comprehensive characteristic indicators are compared with preset thresholds to determine whether there is a collision and / or contact.

7. The mechanical component collision detection method based on multi-information fusion according to claim 6, characterized in that, ， 。 8. The mechanical component collision detection method based on multi-information fusion according to claim 1, characterized in that, Outlier removal and noise filtering are performed on the joint angle and angular velocity data.

9. A collision detection system for mechanical components based on multi-information fusion, characterized in that, include: Memory is used to store instructions that can be executed by the processor; A processor for executing instructions to implement the mechanical component collision detection method based on multi-information fusion as described in any one of claims 1-8.

10. A computer-readable medium storing computer program code, characterized in that, The computer program code, when executed by a processor, implements the mechanical component collision detection method based on multi-information fusion as described in any one of claims 1-8.

Images