Robot anomaly collision detection method and related apparatus

Abstract

The application provides a robot abnormal collision detection method and related device, the robot collects multi-source data composed of robot body vision, environment vision and motion data, generates robot body and environment obstacle point cloud data through time sequence alignment and preprocessing; particleizes two kinds of point cloud data, obtains corresponding particle sets and configures attributes such as three-dimensional coordinates, motion rate, angular velocity and particle weight, finely represents the robot and the environment state; based on the inter-frame matching algorithm, the time sequence of the body particle set is tracked, the time sequence particle state sequence is generated, and the motion change is continuously and stably tracked; the particle motion state change quantity is calculated, and the dynamic difference is captured; the abnormal collision is judged and the collision information is quantified by combining the spatial interaction relationship of the particle set and the motion change quantity, the identification accuracy and the information integrity are improved; the optimized motion instruction is generated and executed, the collision is quickly responded and safely adjusted, and the collision detection sensitivity, the response timeliness and the operation safety are significantly improved.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a method and related apparatus for detecting abnormal collisions in a robot. Background Technology

[0002] When robots operate remotely, unmanned, and in complex dynamic environments, they are prone to accidental collisions with environmental objects. Timely identification of abnormal collisions and rapid adjustment of actions are crucial to ensuring operational safety and preventing equipment damage.

[0003] Current collision detection primarily relies on contact sensors or conventional visual monitoring. Contact-based solutions are costly in hardware, complex to maintain, susceptible to environmental interference, slow to respond, and only output binary results, making it difficult to support precise motion adjustments. Conventional visual solutions have a single perception dimension, poor reliability, and detection lag, failing to provide detailed collision information and hindering timely and appropriate motion adjustments.

[0004] In summary, existing technologies have significant shortcomings in terms of real-time performance, sensitivity, anti-interference capabilities, and motion adjustment adaptability, and cannot meet the needs of highly dynamic operation scenarios. Summary of the Invention

[0005] This application provides a robot abnormal collision detection method and related device. By using multi-source visual data fusion detection and particle-based temporal tracking, it achieves highly sensitive real-time identification, accurate quantification, and rapid action adjustment of robot abnormal collisions, thereby improving the timeliness and sensitivity of robot collision response.

[0006] In a first aspect, embodiments of this application provide a method for detecting abnormal collisions in a robot. The method includes: collecting multi-source data; performing temporal alignment and preprocessing on the multi-source data to generate robot body point cloud data and environmental obstacle point cloud data, wherein the multi-source data includes robot body visual data, environmental visual data, and robot motion data; performing particleization processing on the robot body point cloud data and the environmental obstacle point cloud data respectively to obtain a robot body particle set and an environmental obstacle particle set, and configuring particle attributes for individual particles in each particle set, wherein the particle attributes include particle three-dimensional coordinates, motion speed, angular velocity, and particle weight; performing temporal state tracking on the robot body particle set based on an inter-frame matching algorithm to generate a temporal particle state sequence; calculating the particle motion state change amount based on the temporal particle state sequence; determining abnormal collision events based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the particle motion state change amount, and quantifying collision information; and generating and executing robot optimized motion commands based on the collision information.

[0007] Secondly, embodiments of this application provide a robot abnormal collision detection device, wherein the robot is used to execute the method described in any of the first aspects. The device includes: an acquisition unit, configured to acquire multi-source data, perform temporal alignment and preprocessing on the multi-source data to generate robot body point cloud data and environmental obstacle point cloud data, wherein the multi-source data includes robot body visual data, environmental visual data, and robot motion data; a processing unit, configured to perform particleization processing on the robot body point cloud data and the environmental obstacle point cloud data respectively to obtain a robot body particle set and an environmental obstacle particle set, and configure particle attributes of individual particles in each particle set, wherein the particle attributes include particle three-dimensional coordinates, motion rate, angular velocity, and particle weight; perform temporal state tracking on the robot body particle set based on an inter-frame matching algorithm to generate a temporal particle state sequence; calculate the particle motion state change amount according to the temporal particle state sequence; determine abnormal collision events based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the particle motion state change amount, and quantify collision information; and generate robot optimized motion commands based on the collision information and issue them for execution.

[0008] Thirdly, embodiments of this application provide a server including a processor, a memory, and one or more programs stored in the memory and configured to be executed by the processor, the programs including instructions for performing steps as described in any of the first aspects.

[0009] As can be seen from the embodiments of this application, the robot collects multi-source data consisting of robot body visual data, environmental visual data, and robot motion data. After time-series alignment and preprocessing, robot body point cloud data and environmental obstacle point cloud data are generated, effectively ensuring the accuracy and quality of data synchronization. The two types of point cloud data are respectively processed into particle sets to obtain robot body particle sets and environmental obstacle particle sets, and particle attributes including three-dimensional coordinates, motion speed, angular velocity, and particle weights are configured to achieve a refined representation of the robot and environmental states. Based on the inter-frame matching algorithm, the robot body particle set is used to perform time-series state tracking to generate a time-series particle state sequence, ensuring continuous and stable tracking of motion states. The particle motion state changes are calculated based on the time-series particle state sequence to accurately capture dynamic motion differences. The spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the particle motion state changes are combined to determine abnormal collision events and quantify collision information, improving the accuracy and completeness of collision recognition. Based on the collision information, robot optimized motion commands are generated and issued for execution, achieving rapid response and safe adjustment, and improving the overall reliability, response timeliness, and operational safety of robot collision detection. Attached Figure Description

[0010] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the embodiments 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.

[0011] Figure 1 A schematic block diagram of the robot structure provided in the embodiments of this application; Figure 2 A structural block diagram of a real-time computing device provided in an embodiment of this application; Figure 3 A flowchart illustrating a robot abnormal collision detection method provided in an embodiment of this application; Figure 4 This is a functional unit structure block diagram of a robot abnormal collision detection device provided in an embodiment of this application. Detailed Implementation

[0012] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present application.

[0013] The terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish different objects, not to describe a specific order. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but in some embodiments includes steps or units not listed, or in some embodiments includes other steps or units inherent to these processes, methods, products, or apparatuses.

[0014] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0015] In the embodiments of this application, "and / or" describes the relationship between associated objects, indicating that three relationships can exist. For example, A and / or B can represent the following three situations: A exists alone; A and B exist simultaneously; B exists alone. Among them, A and B can be singular or plural.

[0016] In this embodiment, the symbol " / " can indicate that the preceding and following objects are in an "or" relationship. Alternatively, the symbol " / " can also represent a division sign, i.e., performing a division operation. For example, A / B can mean A divided by B.

[0017] In the embodiments of this application, "at least one item" or its similar expression refers to any combination of these items, including any combination of a single item or a plurality of items. "One or more" means one or more, while "multiple" means two or more. For example, "at least one item" of a, b, or c can represent the following seven cases: a, b, c; a and b; a and c; b and c; a, b, and c. Each of a, b, and c can be an element or a set containing one or more elements.

[0018] In the embodiments of this application, "equal to" can be used with "greater than" and is applicable to technical solutions used when "greater than" is used; it can also be used with "less than" and is applicable to technical solutions used when "less than" is used. When "equal to" is used with "greater than", it is not used with "less than"; when "equal to" is used with "less than", it is not used with "greater than".

[0019] To address the aforementioned technical deficiencies, this application proposes a robot abnormal collision detection scheme. Through a collaborative design of multi-source visual fusion, particle-based modeling, and temporal tracking, it optimizes the entire chain from perception, modeling, tracking, judgment, and control. This application simultaneously collects robot body vision, environmental vision, and motion data. After temporal alignment and preprocessing, it generates body point clouds and obstacle point clouds. The two types of point clouds are then particle-based, and attributes such as 3D coordinates, motion rate, angular velocity, and particle weights are configured to finely characterize the robot and environmental states. Based on an inter-frame matching algorithm, the body particle set is temporally tracked, generating a temporal particle state sequence and calculating motion state changes. Abnormal collision events are determined and collision information is quantified by combining the spatial interaction relationships between particles and the motion changes. Finally, optimized motion commands are generated and executed based on the collision information. This achieves highly sensitive real-time identification, accurate quantification, and rapid action adjustment of abnormal collisions, effectively improving collision detection reliability, response timeliness, and operational safety.

[0020] The technical solution of this application will be described in detail below with reference to specific embodiments.

[0021] Please see Figure 1 , Figure 1 The schematic block diagram of the robot provided in the embodiments of this application is as follows: Figure 1As shown, the robot of this application includes a multi-source visual perception device, a real-time computing device, a motion control device, and a storage device; wherein, the multi-source visual perception device, the motion control device, and the storage device are all electrically connected to the real-time computing device, thereby forming a complete hardware data transmission and control link.

[0022] A multi-source vision perception device is used to collect robot body vision data, environmental vision data and motion data, and output the time-aligned multi-source data to a real-time computing device; in specific implementation, it includes a body vision unit, an environmental vision unit and a synchronization triggering unit.

[0023] The body vision unit is used to collect visual information and motion state data of the robot body; in specific implementation, RGB sensors deployed at the robot's end effector and joints are used. The D-type depth camera has a built-in inertial measurement unit. The depth camera is used to acquire color images and depth information of the robot body, while the inertial measurement unit is used to acquire angular velocity and acceleration data.

[0024] The environmental vision unit is used to collect visual information and obstacle distribution data of the work environment; in practice, it adopts fixed-view RGB sensors deployed around the work scene. D-depth camera.

[0025] The synchronization triggering unit is used to unify the timing of multi-source data acquisition and ensure data synchronization accuracy; in specific implementation, the PTPv2 hardware synchronization module can be used.

[0026] The real-time computing device receives multi-source data and performs time-series alignment, preprocessing, particle modeling, time-series state tracking, collision event determination, and collision information quantification. It then generates optimized motion commands and sends them to the motion control device. Simultaneously, it receives robot joint encoder angle data from the motion control device and stores the raw data, joint angle data, intermediate results, and collision logs in the storage device. In specific implementation, an edge computing platform, such as the NVIDIA Jetson AGXOrin embedded GPU, is used, which has parallel computing capabilities and can achieve millisecond-level data processing and collision detection.

[0027] In one possible embodiment, please refer to Figure 2 , Figure 2 A structural block diagram of a real-time computing device provided in an embodiment of this application is shown below. Figure 2 As shown, the real-time computing device 2 includes a processor 21, a memory 23, a communication interface 22, and one or more programs 231. The one or more programs 231 are stored in the memory 23 and configured to be executed by the processor 21. The one or more programs 231 include instructions for performing any step in an embodiment of a robot abnormal collision detection method described below.

[0028] The motion control device is used to receive optimized motion commands and drive the robot's joints to perform motion adjustments. In specific implementation, it includes a robot controller, servo drive unit and high-speed communication interface, and uses EtherCAT bus to achieve low-latency command transmission, quickly respond to collisions and adjust the motion trajectory.

[0029] The storage device is used to cache multi-source visual data, calculation process data, and collision event records to ensure data traceability. In specific implementation, NVMe high-speed solid-state storage modules are used for data caching, result storage, and log recording to ensure data reliability.

[0030] Based on the above hardware structure, this application proposes a robot abnormal collision detection method according to an embodiment.

[0031] Please see Figure 3 , Figure 3 This is a flowchart illustrating a robot abnormal collision detection method provided in an embodiment of this application. The method is applied to, for example... Figure 1 The robot shown in the figure, the method includes the following steps S301-S306: Step S301: Collect multi-source data, perform time-series alignment and preprocessing on the multi-source data, and generate robot body point cloud data and environmental obstacle point cloud data.

[0032] The multi-source data includes robot body visual data, environmental visual data, and robot motion data.

[0033] In this step, multi-source data is acquired through a multi-source visual perception device in conjunction with the robot's internal encoder: the robot's body visual data is obtained from the RGB values ​​of the body visual unit. The depth camera acquires color images and depth information of the end effector and joints; environmental vision data is obtained from the fixed-view RGB of the environmental vision unit. The D-depth camera acquires color images of the work scene, depth information, and spatial distribution information of obstacles; the robot's motion data is acquired by the inertial measurement unit built into the body vision unit and the robot joint encoder. The inertial measurement unit outputs angular velocity and acceleration data, and the joint encoder acquires robot joint angle data in real time.

[0034] In one possible embodiment, the multi-source data is time-aligned and preprocessed to generate robot body point cloud data and environmental obstacle point cloud data, specifically including: Time alignment: based on unified timestamp The multi-source data is classified and aligned to obtain a time-series data set. ,in Data synchronization deviation is controlled within 1 millisecond to ensure that the data from each sensor are strictly consistent in the time dimension.

[0035] in, Color image data of the robot body; For robot body depth image data; Color image data of the working environment; For depth image data of the working environment; Angular velocity data collected by the inertial measurement unit; Acceleration data collected by the inertial measurement unit; For robot joint angle data; This is the start time of data collection.

[0036] Depth map denoising: Bilateral filtering denoising is performed on both the robot body and the environment depth maps. Let the original depth map be at pixel coordinates... The depth value at that location is Denoising depth value Calculate using the following formula:

[0037] in, For pixels of Neighborhood set; These are normalization coefficients to ensure that the depth values ​​remain consistent before and after filtering. The standard deviation of the spatial domain controls the spatial weight decay of neighboring pixels, and its value is 20. The depth domain standard deviation controls the weighted decay of depth value differences among neighboring pixels, with a value of 0.15.

[0038] This processing method combines spatial distance and depth value differences to perform a weighted average of neighboring pixels, which can smooth image noise and preserve edge information.

[0039] Image segmentation and point cloud extraction: The U2Net model is used to segment the image, obtaining binary masks for the robot body and environmental obstacles. and The mask value of 1 represents the target region. Point cloud data is extracted from the corresponding depth map based on the binary mask to generate the robot body point cloud. Point clouds of environmental obstacles .

[0040] Coordinate System 1: Based on a pre-calibrated offline hand-eye calibration matrix The robot body point cloud is transformed from the camera coordinate system to the robot base coordinate system. This enables the use of a unified coordinate system between the body point cloud and the environment point cloud.

[0041] Finally, this step outputs the calibrated point cloud of the robot body. Environmental obstacle point cloud The time-aligned image and motion data sets provide the foundational data for subsequent collision detection processing.

[0042] Step S302: Perform particle processing on the robot body point cloud data and the environmental obstacle point cloud data respectively to obtain robot body particle set and environmental obstacle particle set, and configure the particle attributes of individual particles in each particle set.

[0043] The particle attributes include the particle's three-dimensional coordinates, motion speed, angular velocity, and particle weight.

[0044] A uniform sampling method is used to discretize the point cloud into particles, transforming the continuously distributed point cloud into a finite number of particle units to reduce computational load and standardize data representation. This is applied to the normalized robot body point cloud. Point clouds with environmental obstacles Particle sampling and attribute assignment are performed separately.

[0045] The particle sampling process uses a fixed sampling interval to filter valid points and generate a discrete particle set. The sampling expression is as follows: ; in, This is the collection of particles generated after sampling; For the set of the first The three-dimensional spatial coordinates of each particle; The original input point cloud; The particle sampling interval is set to 5 mm in this embodiment to ensure uniform particle distribution and non-overlapping.

[0046] Furthermore, each particle is assigned multi-dimensional physical properties, and the attribute vector of a single particle is represented as follows: ; in, The particle's three-dimensional spatial coordinates; This refers to the real-time velocity of the particles. The particle's angular velocity; The particle weights are used to characterize the importance of local structures.

[0047] The specific calculation methods for each particle attribute are as follows: Particle three-dimensional spatial coordinates : Directly taken from the robot body point cloud after calibration Environmental obstacle point cloud It preserves the three-dimensional spatial coordinates of the point cloud itself, eliminating the need for secondary calculations and accurately representing the spatial position of particles. Real-time velocity of particles Movement speed The real-time velocity of the particle is calculated by synthesizing the resultant acceleration in space using triaxial acceleration data collected by the inertial measurement unit. The calculation formula is as follows: ;in , , These are the three-axis axial accelerations output by the inertial measurement unit; Particle angular velocity : Directly use the angular velocity data output in real time from the inertial measurement unit to match the joint position of the particle and characterize the particle's rotational motion state; Particle weight This embodiment can employ both equal-weight and unequal-weight allocation methods, which can be flexibly selected according to the operational scenario. The unequal-weight allocation method assigns weights based on the relative distance of particles to the robot joints; the closer the particle is to the joint and the higher its structural sensitivity, the greater its weight. The calculation formula is as follows: , This is the weight decay coefficient. The distance between the particle and the nearest joint is the straight-line distance. As a preferred embodiment, this example uses an equal-weight allocation method, without assigning values ​​based on differences in particle spatial position. The particle weights of the robot body satisfy... ,in The total number of particles in the robot body; the particle weights of environmental obstacles satisfy the following conditions. ,in This represents the total number of environmental obstacle particles.

[0048] Among them, the environmental obstacles are stationary and have no motion characteristics. Therefore, the motion speed and angular velocity of the obstacle particles are uniformly assigned to 0, and the calculation methods of the other spatial coordinates and weight attributes are consistent with those of the robot body particles.

[0049] This step ultimately generates a collection of robot particles with multidimensional physical properties. and environmental obstacle particle collection The structured modeling is completed, providing a particle basis for subsequent temporal tracking and collision determination. After completing the particle modeling, static obstacle particles do not require continuous dynamic monitoring, while the robot body is in continuous motion. The particle state at a single moment cannot reflect the characteristics of motion disturbances and collision mutations. Therefore, it is necessary to continuously track the dynamic robot body particles in time to capture the motion change patterns of particles over time, providing dynamic temporal data for subsequent collision anomaly determination, and then performing the following temporal tracking steps.

[0050] Step S303: Based on the inter-frame matching algorithm, perform temporal state tracking on the robot body particle set to generate a temporal particle state sequence.

[0051] The temporal particle state sequence is generated from the robot's own particle set. The sequence contains complete particle attributes of the particles at consecutive sampling times, fully recording the dynamic changes of the robot's own particle set during operation. This application selects only the robot's own particle set for temporal tracking, excluding environmental obstacle particles. This is because obstacles are mostly static objects whose spatial positions and motion attributes do not change dynamically over time, thus requiring no continuous temporal tracking. In contrast, the robot itself is in continuous motion; collision anomalies and motion disturbances are reflected in the sudden changes in the motion parameters of the particles. Temporal tracking of the particles alone reduces algorithm computational overhead, improves real-time processing speed, and accurately focuses on the robot's own motion change characteristics, avoiding redundant interference from static obstacle data.

[0052] In one possible embodiment, in conjunction with the aforementioned logic, this application employs a nearest neighbor matching algorithm to complete the association matching of body particles between adjacent frames, and solves the motion change amount based on the spatial position difference of the same particle at consecutive time points.

[0053] In specific implementations, in some embodiments, the step of tracking the temporal state of the robot body particle set based on the inter-frame matching algorithm to generate a temporal particle state sequence includes: using the robot body particle set of the initial frame as the initial state of the temporal particle state sequence; starting from the second frame, using the robot body particle set determined in the previous frame as the reference set and the robot body particle set to be matched in the current frame as the set to be matched; using the inter-frame matching algorithm to optimize the matching between the set to be matched and the reference set to determine the optimal correspondence between the two particle sets; updating the particle attributes of each particle in the set to be matched according to the optimal correspondence to obtain the updated robot body particle set of the current frame; adding the robot body particle set of the current frame to the temporal particle state sequence; repeating the steps "using the robot body particle set determined in the previous frame as the reference set and the robot body particle set to be matched in the current frame as the set to be matched" and subsequent steps until all frame data is processed to obtain a continuous temporal particle state sequence.

[0054] In this embodiment, the initial frame of robot body particles acquired is used as the starting reference state for the temporal particle state sequence. During subsequent continuous frame processing, the previous frame's optimized and calibrated particle set with a determined state is always used as the reference benchmark, and the current frame's particle set generated in real-time is used as the target for optimization. To accurately achieve the correlation matching between particle sets in two frames, this application further employs an iterative nearest-point matching optimization method to iteratively optimize and solve the reference set and the set to be matched, clarifying the optimal mapping relationship between particles in different frames. This matching process includes a complete computational flow such as particle proximity search, shortest distance solution, pose transformation inverse solution, and iterative error convergence determination. After completing the optimal matching, the parameters of the particle set to be matched are corrected and optimized by combining the particle attribute synchronous update logic. After the single-frame particle set is optimized, it is included in the temporal particle state sequence for storage and recording. The above-mentioned inter-frame matching, attribute update, and sequence storage process is iteratively executed until all acquired frames are processed, ultimately generating a continuous and complete temporal particle state sequence. ;in Representing the The frame-optimized body particle set The number of body particle samples set for this application.

[0055] In some embodiments, the step of employing an inter-frame matching algorithm to optimize the matching of the set to be matched with the reference set and determine the optimal correspondence between the two particle sets includes: for each particle in the set to be matched, finding the nearest particle in the reference set to generate a preliminary particle correspondence; calculating a pose adjustment relationship that minimizes the overall positional difference between the two particle sets based on the preliminary particle correspondence; adjusting the overall position and orientation of the set to be matched based on the pose adjustment relationship; repeating the step "for each particle in the set to be matched, finding the nearest particle in the reference set to generate a preliminary particle correspondence" and subsequent steps until the overall positional difference is less than a preset threshold; and outputting the target particle correspondence as the optimal correspondence.

[0056] This embodiment is a detailed execution scheme for the matching stage in the aforementioned time-series tracking process. It employs an iterative nearest-neighbor algorithm to complete particle set registration, used to solve for the optimal correspondence between particle sets in two frames. It fully includes four parts: neighbor particle search, Euclidean distance calculation, pose matrix inverse solution, and iterative convergence determination. The specific implementation process is as follows: (1) Particle proximity search and shortest distance calculation: Traverse all particles in the set to be matched, search for the particle with the closest spatial distance in the reference particle set, and construct particle pairing relationships. The shortest distance between individual particles is determined by three-dimensional Euclidean distance, and the calculation formula is as follows: ; In the formula: The three-dimensional Euclidean distance between the particle to be matched and the reference particle; The three-dimensional coordinates of the particle to be matched The reference particle's three-dimensional coordinates are used. The above formula is used to iterate and solve the problem, selecting the particle with the smallest distance as the paired particle, thus generating an initial particle pairing set.

[0057] (2) Inverse solution of pose adjustment relationship: After determining the initial pairing relationship, in order to minimize the overall deviation of the two sets of particles, the optimal pose adjustment relationship is solved in reverse; in this embodiment, the pose adjustment relationship is mathematically represented as a rigid body transformation matrix. ,in For rotation matrix, Let be the translation vectors, representing the rotational attitude change and spatial position offset of the particle ensemble, respectively. The loss function is the sum of squared distances between all paired particles, expressed as: ; In the formula, No. The frame-optimized set of body particles; The set of body particles to be optimized in the current frame; A single ontological particle in the set to be optimized; No. Frame reference body particle set; A single body particle in the reference set; The L2 norm is used to solve for the Euclidean distance in space.

[0058] The loss function is solved inversely using Singular Value Decomposition (SVD). Under the constraint of minimizing the particle ensemble error, the optimal pose adjustment relationship is obtained by reverse calculation. This pose adjustment relationship is mathematically represented as a rigid body transformation matrix, specifically including two types of parameters: rotation matrix and translation vector. The specific solution logic and corresponding calculation formulas are as follows: The first step is to calculate the centroid coordinates of the two pairs of particles. Centroid centering is then used to eliminate translational offset interference between the particle sets, retaining only the relative attitude changes. (Reference particle set centroid) Centroid of the set of particles to be matched The calculation formula is: ; In the formula, The coordinates of the centroid of the reference particle ensemble; The coordinates of the centroid of the set of particles to be matched; Total number of samples of the main particles.

[0059] Coordinate centering is performed based on the centroid, and the particle coordinates after centering satisfy: ; In the formula, The coordinates of the reference particle after centralization; The coordinates of the particles to be matched after centralization.

[0060] The second step is to construct the covariance matrix based on the centered particle coordinates. This is used to characterize the spatial correlation between the reference particle and the particle to be matched, and the formula is as follows: ; In the formula, The particle covariance matrix represents the degree of spatial correlation between two groups of particles.

[0061] The third step is to perform singular value decomposition on the covariance matrix. The decomposition formula is as follows: ; In the formula, , It is an orthogonal matrix. Let be a singular value diagonal matrix. The optimal rotation matrix is ​​derived based on orthogonal matrix derivation, and the solution formula is: ; In the formula, The rotation matrix represents the attitude deflection in the pose adjustment relationship; Translation vectors represent the spatial offset in pose adjustment relationships.

[0062] The fourth step involves combining the difference in the centroid offset between the two groups of particles with the obtained rotation matrix to calculate the translation vector in reverse. The formula for calculating the translation vector is: ; This translation vector is used to characterize the spatial offset in the pose adjustment relationship. Finally, the rotation matrix is ​​integrated. With translation vector Generate the rigid body transformation matrix that minimizes the error of the loss function, thus obtaining the optimal pose adjustment relationship.

[0063] (3) Particle set update and iterative convergence judgment: The solved pose adjustment relationship (rigid body transformation matrix) is applied to the particle set to be matched to complete the overall spatial position correction of the particles; after the correction is completed, neighbor particle matching is performed again, and the optimization is iterated repeatedly. The set average error is defined as the convergence judgment index: ; When the average error If the error is less than the preset error threshold (the threshold is set to 0.2 mm in this embodiment), the registration is considered complete, the iteration is terminated, and the optimal particle mapping relationship is output.

[0064] In some embodiments, updating the particle attributes of each particle in the set to be matched according to the optimal correspondence to obtain the updated current frame robot body particle set includes: obtaining the optimal pose adjustment relationship of the set to be matched relative to the reference set according to the optimal correspondence; performing a global transformation on the three-dimensional coordinates of each particle in the set to be matched according to the optimal pose adjustment relationship to update the three-dimensional coordinates of each particle in the set to be matched; updating the motion rate of each particle in the set to be matched according to the three-axis acceleration of the inertial measurement unit of the robot body in the current frame; updating the angular velocity of each particle in the set to be matched according to the joint angular velocity of the robot in the current frame; and obtaining the updated current frame robot body particle set according to the updated three-dimensional coordinates, resultant velocity, and angular velocity.

[0065] This embodiment follows the iterative matching results described above, serving as the execution logic for updating the optimized particle set attributes. It is the final step in completing the single-frame particle state calibration. After obtaining the optimal particle correspondence and pose adjustment relationship, the pose adjustment relationship is visualized as a rigid body transformation matrix. First, based on this matrix, a global rigid body transformation is performed on the three-dimensional spatial coordinates of the particles to be matched to correct the particle spatial position deviation. The transformation formula is: ; In the formula, The particle coordinates before transformation The transformed particle coordinates are then used. Subsequently, real-time sensor data acquired by the hardware is used to update dynamic attributes, including three-axis acceleration acquired by the inertial acquisition unit. Synthetic computation of real-time particle motion speed Simultaneously, the particle angular velocity is updated synchronously using the angular velocity data fed back from the robot's joint encoder. The particle weights remain equally distributed, satisfying... By integrating and correcting the spatial coordinates, motion rate, angular velocity, and weights, a complete and accurate set of particles for the current frame is generated, completing a single temporal tracking iteration.

[0066] To further clarify the execution flow of this algorithm, a complete example of single-frame tracking is provided: setting the acquisition timing interval. The reference particle set for the previous frame is The set of particles to be matched in the current frame is First, a three-dimensional Euclidean distance formula was used to traverse and search, selecting 2000 pairs of corresponding body particles. Then, SVD decomposition was used to solve the pose matrix in reverse, yielding a rotation angle of 0.02 rad and a translation offset of 0.35 mm. The transformation matrix was then applied to the particles to be matched, completing the overall fine-tuning of the particles. Finally, the ensemble average error was iteratively calculated. If the particle size is less than a preset threshold of 0.20 mm, the iteration is considered converged. The particle coordinates are updated based on the optimal transformation result, and the acceleration is collected by the inertial acquisition unit. The resultant velocity of the particles was calculated. Synchronously write joint angular velocity Finally, an updated set of particles for the current frame is generated and stored in the temporal particle state sequence, completing a full temporal tracking process. This example fully reproduces the algorithm's operational logic and can intuitively demonstrate the entire process of particle matching, pose solving, and attribute updating in this scheme.

[0067] As can be seen, this step completes the temporal state tracking of the robot's particle set through an inter-frame matching algorithm, achieves accurate registration of particle sets in adjacent frames using an iterative nearest-point algorithm, and solves the optimal pose adjustment relationship using singular value decomposition to correct the particle's spatial position. Simultaneously, it integrates sensor data collected by the inertial measurement unit and joint encoders to update dynamic attributes such as particle motion rate and angular velocity in real time, constructing a continuous and complete temporal particle state sequence. This step only performs temporal tracking on dynamic body particles, excluding static obstacle particles from participating in iterative calculations. This reduces the algorithm's computational overhead while ensuring particle matching accuracy and motion state update accuracy. Furthermore, through equal weight allocation and iteration error convergence judgment, it further improves the stability and matching accuracy of particle temporal association. The complete temporal particle state sequence preserves the motion evolution characteristics during robot operation, clearly recording the continuous changes in particle spatial position and motion parameters, providing complete temporal data support for the next step of particle collision determination and abnormal disturbance identification, and enabling accurate tracing and quantitative analysis of the robot's dynamic collision characteristics.

[0068] Step S304: Calculate the change in particle motion state based on the time-series particle state sequence.

[0069] This step, based on the previously generated temporal particle state sequence, quantitatively analyzes the motion fluctuations of a single particle at consecutive sampling times. By extracting the particle parameter differences between adjacent frames, it solves for the particle motion state changes, including displacement, velocity change, and angular velocity change. Unlike single-frame static particle parameters, the motion state changes can intuitively characterize the instantaneous motion disturbance of the robot's particles, effectively distinguishing between normal robot operation jitter and parameter mutations caused by collisions, providing quantitative difference indicators for subsequent collision event determination.

[0070] In some embodiments, calculating the particle motion state change based on the temporal particle state sequence includes: sequentially reading the robot body particle sets of two adjacent frames from the temporal particle state sequence; for each particle, calculating the difference in motion rate between the current frame and the previous frame to obtain the velocity change, and calculating the difference in angular velocity between the current frame and the previous frame to obtain the angular velocity change, and calculating the difference in three-dimensional coordinates between the current frame and the previous frame to obtain the displacement; and integrating the velocity change, angular velocity change, and displacement of each particle into the particle motion state change.

[0071] In the specific implementation, the robot's particle sets of two temporally adjacent frames are read sequentially from the temporal particle state sequence, namely the reference particle set of the previous frame and the optimized particle set of the current frame. Based on the optimal correspondence between particle frames, the state parameters of the same particle at different times are matched, and the parameter differences in the spatial dimension, motion dimension, and rotation dimension are calculated respectively. Finally, the complete particle motion state change is obtained by integrating the results. The specific calculation method is as follows: (1) Displacement calculation: Based on the three-dimensional spatial coordinates of the same particle in adjacent frames, the change in spatial displacement of the particle within a single frame time interval is calculated. The calculation formula is as follows: ; In the formula: The three-dimensional coordinates of the particle in the current frame. The three-dimensional coordinates of the particles in the previous frame. This represents the inter-frame displacement of the particles.

[0072] (2) Calculation of velocity change: Combining the motion velocities of particles in consecutive frames, the velocity difference is calculated to characterize the translational motion fluctuations of the particles. The calculation formula is as follows: ; In the formula: The particle motion rate in the current frame. The particle motion rate of the previous frame. This represents the change in particle velocity.

[0073] (3) Calculation of angular velocity change: Match the angular velocity parameters of particles in adjacent frames, solve for the angular velocity difference to characterize the fluctuation of particle rotational motion, and the calculation formula is: ; In the formula: The particle angular velocity in the current frame. The particle angular velocity of the previous frame. This represents the change in the particle's angular velocity.

[0074] By integrating the displacement, velocity change, and angular velocity change of a single particle, a complete set of particle motion state changes is constructed, expressed as follows: ; In the formula: For the first The motion state changes of each of the 2000 robot particles are calculated by iterating through all the particles and solving for the differences in their motion parameters, thus obtaining a dataset of motion changes of all particles in the global domain.

[0075] As can be seen, this step performs difference calculations based on paired particles after temporal association, without the need for additional particle association matching, thus reducing the computational cost of difference calculations. By quantifying particle motion deviations in multiple dimensions, and taking into account three types of motion characteristics—spatial displacement, translational velocity, and rotational angular velocity—it can comprehensively capture minute motion disturbances generated during robot operation, providing a quantitative calculation basis for setting collision judgment thresholds and identifying abnormal collision events.

[0076] Step S305: Determine abnormal collision events based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the change in particle motion state, and quantify the collision information.

[0077] This step combines spatial and motion dimensions for abnormal collision detection. Based on the spatial interaction between the robot's particle set and the obstacle particle set, interactive particles within the potential contact range are selected. Then, combined with the previously calculated particle motion state changes, it determines whether a particle has experienced a sudden motion change, comprehensively judging whether the robot has generated an abnormal collision event. Simultaneously, after a collision is determined, collision information is quantified based on the motion parameters of the abnormal particles, calculating the collision location, collision force magnitude, and collision direction, achieving accurate source tracing and quantitative assessment of the collision event. This step employs a dual-judgment logic of spatial distance pre-screening + motion change verification, effectively avoiding misjudgments caused by environmental noise and normal mechanical vibrations, thus improving the reliability of the collision detection algorithm.

[0078] In some embodiments, determining abnormal collision events based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the change in particle motion state, and quantifying collision information, includes: calculating the spatial distance between each particle in the robot body particle set and each particle in the environmental obstacle particle set in the current frame; selecting robot body particles whose spatial distance is less than a preset distance threshold as near-range interaction particles; comparing the change in particle motion state of the near-range interaction particles with a preset motion anomaly threshold, selecting abnormal particles among the near-range interaction particles whose change in motion state exceeds the motion anomaly threshold, and calculating the proportion of the abnormal particles in the robot body particle set in the current frame; when the proportion of particles exceeds a preset ratio, determining that the robot has experienced the abnormal collision event; identifying the abnormal particles as collision particles, and determining the collision information based on the change in motion state of the collision particles.

[0079] In the specific implementation process, the spatial distance between the robot's own particles and environmental obstacle particles in the current frame is first calculated to determine the interaction relationship between particles. The spatial distance between any robot particle and obstacle particles is solved using the three-dimensional Euclidean distance formula, and the calculation formula is as follows: ; In the formula: The three-dimensional coordinates of the particle in the current frame. The three-dimensional coordinates of the obstacle particle. This represents the spatial distance between the main particle and the obstacle particles. The spatial distance is compared with a preset distance threshold, and the main particles whose spatial distance is less than the preset distance threshold are selected and defined as close-range interaction particles; these particles represent particles that are currently in the vicinity of the obstacle and have the potential risk of collision.

[0080] After identifying nearby interactive particles, anomaly detection is performed based on changes in particle motion state. The changes in motion state corresponding to the nearby interactive particles are compared with a preset motion anomaly threshold. Particles whose changes in motion state exceed the threshold are identified as anomalous particles, exhibiting sudden motion disturbances while close to obstacles in space. The number of anomalous particles is counted, and their proportion in the current frame's main particle set is calculated using the following formula: ; In the formula: The percentage of abnormal particles, The number of anomalous particles, This represents the total number of particles in the robot. When the proportion of abnormal particles exceeds a preset threshold, single-point noise disturbances are excluded, and the robot is determined to have experienced an abnormal collision event. These abnormal particles are then marked as collision particles, serving as the calculation carrier for subsequent collision information quantification.

[0081] In some embodiments, determining the collision information based on the change in the motion state of the colliding particle includes: averaging the three-dimensional coordinates of the colliding particle to obtain the three-dimensional collision position; calculating the magnitude of the collision force based on the change in the velocity of the colliding particle and the equivalent mass of the robot's end effector; summing and normalizing the displacement of the colliding particle to obtain the collision direction; and integrating the three-dimensional collision position, the magnitude of the collision force, and the collision direction into the collision information.

[0082] After determining the set of colliding particles, multidimensional collision information is quantified based on the changes in the motion state of the colliding particles. The three-dimensional collision position, collision force magnitude, and collision direction are calculated separately, and then integrated to obtain complete collision information. The specific quantization method is as follows: (1) Calculation of three-dimensional collision position: The arithmetic mean of the three-dimensional spatial coordinates of all colliding particles is calculated, and the particle cluster center is taken as the actual collision location to avoid the sampling error of a single particle. The calculation formula is as follows: ; In the formula: These are the three-dimensional collision position coordinates. The coordinates of a single colliding particle are 3D.

[0083] (2) Calculation of collision force: Based on the principle of momentum change, the collision force is calculated by combining the change in the velocity of the colliding particles with the equivalent mass of the robot's end effector. The average momentum change is used to characterize the overall collision impact intensity. The calculation formula is as follows: ; In the formula: The magnitude of the collision force, For the equivalent mass of the robot end effector, This represents the change in the velocity of the colliding particles.

[0084] (3) Collision direction calculation: The inter-frame displacement of colliding particles is vector-summed, and then normalized to obtain the unit direction vector, which represents the force direction of the collision impact. The calculation formula is: ; In the formula: This is the normalized collision direction vector. This represents the inter-frame displacement vector of the colliding particles.

[0085] Finally, the obtained 3D collision position, collision force magnitude, and collision direction are integrated to construct structured collision information, thus completing the detection and quantification of this abnormal collision event.

[0086] As can be seen, this step significantly reduces the probability of false detection through dual discrimination of spatial proximity constraints and motion mutation constraints; at the same time, it relies on particle cluster computing to solve collision parameters, which has stronger anti-interference ability compared with single-point detection method, and can accurately and stably output collision quantification data, providing reliable data support for subsequent robot collision damping control, motion trajectory reversal, and safe obstacle avoidance strategy execution.

[0087] Step S306: Generate robot optimized motion instructions based on the collision information and issue them for execution.

[0088] This step, based on the collision information quantified in the previous steps, adaptively adjusts the robot's motion control strategy. Combining three types of collision parameters—three-dimensional collision position, collision force magnitude, and collision direction—it corrects the robot's original trajectory from three dimensions: motion orientation, motion speed, and working position. This generates optimized motion commands that meet safe operating requirements and sends them to the robot's execution end to complete the action response. This step achieves closed-loop control from collision detection to safety protection, adjusting motion parameters according to the actual collision conditions to avoid motion stuttering and excessive avoidance issues caused by fixed protection strategies. It ensures robot operational safety while maintaining reasonable operational efficiency.

[0089] In some embodiments, generating optimized motion commands for the robot based on the collision information includes: acquiring the collision information, which includes a three-dimensional collision position, a collision force magnitude, and a collision direction; determining an avoidance direction for the robot's end effector based on the collision direction, wherein the avoidance direction is a safe movement direction opposite to or perpendicular to the collision direction; determining a deceleration magnitude based on the collision force magnitude, wherein the deceleration magnitude is positively correlated with the collision force magnitude; and generating the optimized motion commands by combining the three-dimensional collision position, the avoidance direction, and the deceleration magnitude.

[0090] In the specific implementation process, the structured collision information is first read, which includes three core parameters: three-dimensional collision position, collision force magnitude, and collision direction. Then, the avoidance direction is determined, the deceleration magnitude is matched, and finally, spatial position constraints are fused to generate optimized motion commands. The specific implementation logic is as follows: (1) Determining the avoidance direction: Based on the quantified collision direction, the safe avoidance direction of the robot's end effector is determined. To ensure the robot quickly escapes the dangerous contact area, the avoidance direction is set to a movement direction opposite to or perpendicular to the collision direction; among which, reverse movement is used to deal with rigid direct impact conditions, and vertical lateral movement is used to avoid scraping and side-extrusion conditions. The avoidance direction vector is defined as follows: The decision logic satisfies: ; In the formula: It is the opposite vector to the direction of the collision. This is a lateral vector perpendicular to the collision direction. By determining the differentiated direction, the robot can select the optimal escape path based on the collision force.

[0091] (2) Determination of deceleration amplitude: The deceleration amplitude is matched to the magnitude of the collision force. The deceleration amplitude is positively correlated with the magnitude of the collision force, that is, the greater the collision force, the greater the degree of deceleration of the robot's motion, thereby suppressing mechanical vibration and the risk of secondary collisions under strong impacts. In this embodiment, a linear correlation model is used to construct the deceleration coefficient, and the calculation formula is as follows: ; In the formula: The speed reduction coefficient has a value range of (0,1). This is the intensity-related adjustment coefficient; To detect collision force in real time; The system is preset with a maximum safe collision force. The greater the collision force, the smaller the deceleration coefficient, and the lower the robot's end effector speed, thus achieving adaptive deceleration control based on the force.

[0092] (3) Optimized motion command generation: Optimized motion commands are generated by combining the three-dimensional collision position, avoidance direction, and deceleration magnitude. The three-dimensional collision position is used as the motion control point to restrict the robot from continuing to move into the danger zone; the avoidance direction is used as the trajectory correction direction to drive the robot away from the obstacle; and the deceleration coefficient is used to correct the end effector speed. By combining the three types of constraints, structured optimized motion commands containing position correction, motion direction, and running speed are generated and sent to the robot's servo actuator to complete safety actions such as avoidance deceleration, trajectory reversal, and smooth departure.

[0093] As can be seen, this step relies on quantified collision information to achieve graded and differentiated motion optimization and control. Compared with the traditional single emergency stop protection method, this solution can accurately adapt to operation scenarios with different collision intensities and collision directions. Through the combination of flexible deceleration and directional avoidance control, the instantaneous impact on the mechanical structure is reduced, the service life of the robot is extended, and autonomous safety avoidance after collision is achieved, thus improving the robot's abnormal collision protection system.

[0094] The above primarily describes the solutions of the embodiments of this application from the perspective of the method execution process. It is understood that, in order to achieve the above functions, the server includes the corresponding hardware structure and / or software modules for executing each function. Those skilled in the art should readily recognize that, based on the units and algorithm steps of the examples described in the embodiments provided herein, this application can be implemented in hardware or a combination of hardware and computer software. Whether a function is executed in hardware or by computer software driving hardware depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0095] This application embodiment can divide the server into functional units according to the above method example. For example, each function can be divided into different functional units, or two or more functions can be integrated into one processing module. The integrated unit can be implemented in hardware or as a software program module. It should be noted that the unit division in this application embodiment is illustrative and only represents a logical functional division, while other division methods may be used in actual implementation.

[0096] In the case of using integrated units, please refer to Figure 4 , Figure 4 This application provides a functional unit structure block diagram of a robot abnormal collision detection device 4, which includes: The acquisition unit 401 is used to collect multi-source data, perform time-series alignment and preprocessing on the multi-source data, and generate robot body point cloud data and environmental obstacle point cloud data. The multi-source data includes robot body visual data, environmental visual data and robot motion data. The processing unit 402 is configured to perform particle-based processing on the robot body point cloud data and the environmental obstacle point cloud data respectively, to obtain a robot body particle set and an environmental obstacle particle set, and to configure the particle attributes of individual particles in each particle set, including particle three-dimensional coordinates, motion speed, angular velocity, and particle weight; to perform temporal state tracking on the robot body particle set based on an inter-frame matching algorithm, generating a temporal particle state sequence; to calculate the particle motion state change based on the temporal particle state sequence; to determine abnormal collision events based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the particle motion state change, and to quantify collision information; and to generate and execute robot optimized motion commands based on the collision information.

[0097] As can be seen from the embodiments of this application, the robot collects multi-source data consisting of robot body visual data, environmental visual data, and robot motion data. After time-series alignment and preprocessing, robot body point cloud data and environmental obstacle point cloud data are generated, effectively ensuring the accuracy and quality of data synchronization. The two types of point cloud data are respectively processed into particle sets to obtain robot body particle sets and environmental obstacle particle sets, and particle attributes including three-dimensional coordinates, motion speed, angular velocity, and particle weights are configured to achieve a refined representation of the robot and environmental states. Based on the inter-frame matching algorithm, the robot body particle set is used to perform time-series state tracking to generate a time-series particle state sequence, ensuring continuous and stable tracking of motion states. The particle motion state changes are calculated based on the time-series particle state sequence to accurately capture dynamic motion differences. The spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the particle motion state changes are combined to determine abnormal collision events and quantify collision information, improving the accuracy and completeness of collision recognition. Based on the collision information, robot optimized motion commands are generated and issued for execution, achieving rapid response and safe adjustment, and improving the overall reliability, response timeliness, and operational safety of robot collision detection.

[0098] In some embodiments, in tracking the temporal state of the robot body particle set based on an inter-frame matching algorithm to generate a temporal particle state sequence, the processing unit 402 is configured to: use the robot body particle set of the initial frame as the initial state of the temporal particle state sequence; starting from the second frame, use the robot body particle set determined in the previous frame as the reference set and the robot body particle set to be matched in the current frame as the set to be matched; use an inter-frame matching algorithm to optimize the matching between the set to be matched and the reference set to determine the optimal correspondence between the two particle sets; update the particle attributes of each particle in the set to be matched according to the optimal correspondence to obtain the updated robot body particle set of the current frame; add the robot body particle set of the current frame to the temporal particle state sequence; repeat the steps "using the robot body particle set determined in the previous frame as the reference set and the robot body particle set to be matched in the current frame as the set to be matched" and the subsequent steps until all frame data is processed to obtain a continuous temporal particle state sequence.

[0099] In some embodiments, in determining the optimal correspondence between two particle sets by employing an inter-frame matching algorithm to optimize the matching between the set to be matched and the reference set, the processing unit 402 is configured to: for each particle in the set to be matched, find the nearest particle in the reference set and generate a preliminary particle correspondence; calculate the pose adjustment relationship that minimizes the overall position difference between the two particle sets based on the preliminary particle correspondence; adjust the overall position and orientation of the set to be matched based on the pose adjustment relationship; repeat the steps "for each particle in the set to be matched, find the nearest particle in the reference set and generate a preliminary particle correspondence" and subsequent steps until the overall position difference is less than a preset threshold; and output the target particle correspondence as the optimal correspondence.

[0100] In some embodiments, in updating the particle attributes of each particle in the set to be matched according to the optimal correspondence to obtain the updated current frame robot body particle set, the processing unit 402 is configured to: obtain the optimal pose adjustment relationship of the set to be matched relative to the reference set according to the optimal correspondence; perform an overall transformation on the three-dimensional coordinates of each particle in the set to be matched according to the optimal pose adjustment relationship, and update the three-dimensional coordinates of each particle in the set to be matched; update the motion rate of each particle in the set to be matched according to the three-axis acceleration of the inertial measurement unit of the robot body in the current frame; update the angular velocity of each particle in the set to be matched according to the joint angular velocity of the robot in the current frame; and obtain the updated current frame robot body particle set according to the updated three-dimensional coordinates, resultant velocity, and angular velocity.

[0101] In some embodiments, in calculating the change in particle motion state based on the temporal particle state sequence, the processing unit 402 is configured to: sequentially read the robot body particle sets of two adjacent frames from the temporal particle state sequence; for each particle, calculate the difference in motion rate between the current frame and the previous frame to obtain the change in velocity, and calculate the difference in angular velocity between the current frame and the previous frame to obtain the change in angular velocity, and calculate the difference in three-dimensional coordinates between the current frame and the previous frame to obtain the displacement; and integrate the change in velocity, the change in angular velocity, and the displacement of each particle into the change in particle motion state.

[0102] In some embodiments, in determining abnormal collision events and quantifying collision information based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the change in particle motion state, the processing unit 402 is configured to: calculate the spatial distance between each particle in the robot body particle set and each particle in the environmental obstacle particle set in the current frame; filter out robot body particles whose spatial distance is less than a preset distance threshold as near-range interaction particles; compare the change in particle motion state of the near-range interaction particles with a preset motion anomaly threshold, filter out abnormal particles among the near-range interaction particles whose change in motion state exceeds the motion anomaly threshold, and calculate the proportion of the abnormal particles in the robot body particle set in the current frame; when the proportion of particles exceeds a preset ratio, determine that the robot has experienced the abnormal collision event; identify the abnormal particles as collision particles, and determine the collision information based on the change in motion state of the collision particles.

[0103] In some embodiments, in determining the collision information based on the change in the motion state of the colliding particle, the processing unit 402 is configured to: average the three-dimensional coordinates of the colliding particle to obtain the three-dimensional collision position; calculate the magnitude of the collision force based on the change in the velocity of the colliding particle and the equivalent mass of the robot end effector; sum and normalize the displacement of the colliding particle to obtain the collision direction; and integrate the three-dimensional collision position, the magnitude of the collision force, and the collision direction into the collision information.

[0104] In some embodiments, in generating optimized motion commands for the robot based on the collision information, the processing unit 402 is configured to: acquire the collision information, the collision information including a three-dimensional collision position, a collision force magnitude, and a collision direction; determine an avoidance direction for the robot's end effector based on the collision direction, the avoidance direction being a safe movement direction opposite to or perpendicular to the collision direction; determine a deceleration magnitude based on the collision force magnitude, the deceleration magnitude being positively correlated with the collision force magnitude; and generate the optimized motion commands by combining the three-dimensional collision position, the avoidance direction, and the deceleration magnitude.

[0105] This application provides a computer-readable storage medium storing a computer program / instructions thereon, which, when executed by a processor, implement the steps of any possible embodiment of the method.

[0106] It should be noted that, for the sake of simplicity, the foregoing method embodiments are all described as a series of actions. However, those skilled in the art should understand that this application is not limited to the described order of actions, as some steps may be performed in other orders or simultaneously according to this application. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are preferred embodiments, and the actions and modules involved are not necessarily essential to this application.

[0107] In the above embodiments, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

[0108] In the several embodiments provided in this application, it should be understood that the disclosed apparatus can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of the units described above is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical or other forms.

[0109] The units described above as separate components may or may not be physically separate. The components shown as units 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 units can be selected to achieve the purpose of this embodiment according to actual needs.

[0110] Furthermore, the functional units in the various embodiments of this application 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 as a software functional unit.

[0111] If the aforementioned integrated units are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage device (CMD). Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a memory and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned memory includes various media capable of storing program code, such as USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks.

[0112] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage device, which may include: a flash drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, etc.

[0113] The embodiments of this application have been described in detail above. Specific examples have been used to illustrate the principles and implementation methods of this application. The description of the above embodiments is only for the purpose of helping to understand the method and core ideas of this application. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this application. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for detecting abnormal collisions in a robot, characterized in that, The method includes: Collect multi-source data, perform time-series alignment and preprocessing on the multi-source data, and generate robot body point cloud data and environmental obstacle point cloud data. The multi-source data includes robot body visual data, environmental visual data and robot motion data. The robot body point cloud data and the environmental obstacle point cloud data are respectively processed into particles to obtain a robot body particle set and an environmental obstacle particle set. The particle attributes of individual particles in each particle set are configured. The particle attributes include particle three-dimensional coordinates, motion speed, angular velocity and particle weight. The robot body particle set is tracked in a temporal state based on an inter-frame matching algorithm to generate a temporal particle state sequence. Calculate the change in particle motion state based on the temporal particle state sequence; Abnormal collision events are determined based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the change in the particle motion state, and the collision information is quantified. Based on the collision information, generate optimized robot motion commands and issue them for execution; The step of tracking the temporal state of the robot body particle set based on the inter-frame matching algorithm to generate a temporal particle state sequence includes: using the robot body particle set of the initial frame as the initial state of the temporal particle state sequence; starting from the second frame, using the robot body particle set determined in the previous frame as the reference set and the robot body particle set to be matched in the current frame as the set to be matched; using the inter-frame matching algorithm to optimize the matching between the set to be matched and the reference set to determine the optimal correspondence between the two particle sets; updating the particle attributes of each particle in the set to be matched according to the optimal correspondence to obtain the updated robot body particle set of the current frame; adding the robot body particle set of the current frame to the temporal particle state sequence; repeating the steps "using the robot body particle set determined in the previous frame as the reference set and the robot body particle set to be matched in the current frame as the set to be matched" and subsequent steps until all frame data is processed to obtain a continuous temporal particle state sequence; The inter-frame matching algorithm is used to optimize the matching between the set to be matched and the reference set to determine the optimal correspondence between the two particle sets. This includes: for each particle in the set to be matched, finding the nearest particle in the reference set to generate a preliminary particle correspondence; calculating the pose adjustment relationship that minimizes the overall position difference between the two particle sets based on the preliminary particle correspondence; adjusting the overall position and orientation of the set to be matched based on the pose adjustment relationship; repeating the step "for each particle in the set to be matched, finding the nearest particle in the reference set to generate a preliminary particle correspondence" and subsequent steps until the overall position difference is less than a preset threshold; and outputting the target particle correspondence as the optimal correspondence.

2. The method according to claim 1, characterized in that, The step of updating the particle attributes of each particle in the set to be matched according to the optimal correspondence to obtain the updated set of robot body particles in the current frame includes: Based on the optimal correspondence, obtain the optimal pose adjustment relationship of the set to be matched relative to the reference set; Based on the optimal pose adjustment relationship, the three-dimensional coordinates of each particle in the set to be matched are transformed as a whole, and the three-dimensional coordinates of each particle in the set to be matched are updated. The motion rate of each particle in the set to be matched is updated based on the three-axis acceleration of the robot's inertial measurement unit in the current frame. The angular velocities of each particle in the set to be matched are updated based on the robot joint angular velocities in the current frame. The updated set of robot body particles for the current frame is obtained based on the updated 3D coordinates, motion rate, and angular velocity.

3. The method according to claim 1, characterized in that, The step of calculating the change in particle motion state based on the time-series particle state sequence includes: From the temporal particle state sequence, the robot body particle set of two adjacent frames is read sequentially; For each particle, calculate the difference in motion rate between the current frame and the previous frame to obtain the rate change, and... Calculate the difference in angular velocity between the current frame and the previous frame to obtain the change in angular velocity, and... Calculate the difference between the three-dimensional coordinates of the current frame and the previous frame to obtain the displacement. The changes in velocity, angular velocity, and displacement of each particle are integrated into a change in the particle's motion state.

4. The method according to claim 3, characterized in that, The step of determining abnormal collision events based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the change in particle motion state, and quantifying collision information, includes: Calculate the spatial distance between each particle in the robot's body particle set and each particle in the environmental obstacle particle set in the current frame; Robot body particles whose spatial distance is less than a preset distance threshold are selected as near-range interaction particles; The particle motion state change of the near-interaction particles is compared with a preset motion anomaly threshold, and abnormal particles in the near-interaction particles whose motion state change exceeds the motion anomaly threshold are filtered out. The proportion of the abnormal particles in the robot body particle set in the current frame is calculated. When the proportion of the particles exceeds a preset ratio, it is determined that the robot has experienced the abnormal collision event; The abnormal particles are identified as collision particles, and the collision information is determined based on the change in the motion state of the collision particles.

5. The method according to claim 4, characterized in that, Determining the collision information based on the change in the motion state of the colliding particles includes: The three-dimensional collision position is obtained by averaging the three-dimensional coordinates of the colliding particles. The magnitude of the collision force is calculated based on the change in velocity of the colliding particles and the equivalent mass of the robot's end effector. The collision direction is obtained by summing and normalizing the displacement of the colliding particles. The three-dimensional collision location, the magnitude of the collision force, and the collision direction are integrated into the collision information.

6. The method according to claim 1, characterized in that, The step of generating optimized robot motion commands based on the collision information includes: The collision information is obtained, including the three-dimensional collision position, the magnitude of the collision force, and the collision direction; The avoidance direction of the robot end effector is determined based on the collision direction, wherein the avoidance direction is a safe movement direction that is opposite to or perpendicular to the collision direction. The deceleration amplitude is determined based on the magnitude of the collision force, and the deceleration amplitude is positively correlated with the magnitude of the collision force. The optimized motion command is generated by combining the three-dimensional collision position, the avoidance direction, and the deceleration magnitude.

7. A robot abnormal collision detection device, characterized in that, The apparatus for performing the method as described in any one of claims 1-6, the apparatus comprising: The acquisition unit is used to collect multi-source data, perform time-series alignment and preprocessing on the multi-source data, and generate robot body point cloud data and environmental obstacle point cloud data. The multi-source data includes robot body visual data, environmental visual data and robot motion data. The processing unit is used to perform particle-based processing on the robot body point cloud data and the environmental obstacle point cloud data respectively, to obtain a robot body particle set and an environmental obstacle particle set, and to configure the particle attributes of individual particles in each particle set. The particle attributes include particle three-dimensional coordinates, motion speed, angular velocity, and particle weight; to perform temporal state tracking on the robot body particle set based on an inter-frame matching algorithm, generating a temporal particle state sequence; to calculate the particle motion state change based on the temporal particle state sequence; to determine abnormal collision events based on the spatial interaction relationship between the robot body particle set and the environmental obstacle particle set and the particle motion state change, and to quantify collision information; and to generate and execute robot optimized motion commands based on the collision information.

8. A server, characterized in that, The method includes a processor, a memory, and one or more programs stored in the memory and configured to be executed by the processor, the programs including instructions for performing the steps of the method as described in any one of claims 1-6.

Images