Collision detection method and module, robot and related apparatus

Abstract

Embodiments of the present application provide a collision detection method and module, a robot and related devices. The method comprises: obtaining perception data of the robot; calculating a current estimation residual based on an observer; the observer comprises a momentum change term and a residual accumulation term, the momentum change term is calculated based on joint angle and joint angular velocity, and the residual accumulation term is obtained by integrating motor current, current estimation residual and dynamics compensation term; when the current estimation residual is not within a specified numerical range and the duration exceeds a specified duration, the robot is controlled to stop moving, and / or the robot is controlled to maintain the current pose. Since the current estimation residual can reflect the instantaneous deviation between the actual momentum change and the theoretical momentum change, the non-normal momentum change (such as external force generated by collision) can be accurately identified. The method can effectively filter high-frequency noise and inertial disturbance in motion without relying on acceleration measurement, and improve the sensitivity and stability of collision detection.

Description

Technical Field

[0001] This application relates to the field of robotics, and more particularly to collision detection methods and modules, robots, and related devices. Background Technology

[0002] With the widespread application of robots in manufacturing, logistics, and service industries, robot safety has become an increasingly important concern. For example, accurately and promptly detecting collisions when robotic arms move at high speeds or come into contact with the environment or people is a key technology for ensuring the safety of equipment and personnel. However, related collision detection methods have low sensitivity and stability, and usually rely on measurements from accelerometers.

[0003] Based on this, embodiments of this application provide a collision detection method and module, a robot and related devices to improve related technologies. Summary of the Invention

[0004] The purpose of this application is to provide a collision detection method and module, a robot and related devices, to improve the sensitivity and stability of robot collision detection and reduce reliance on acceleration sensors.

[0005] The objective of this application embodiment is achieved using the following technical solutions:

[0006] In a first aspect, embodiments of this application provide a collision detection method applied to a collision detection module in a robot control device, the robot control device further including a task planning and execution module and / or a joint servo control module; the method includes: acquiring sensory data of the robot, the sensory data including joint angles, joint angular velocities, and motor currents; inputting the sensory data into an observer, and calculating a current estimation residual based on the observer; the observer includes a momentum change term and a residual accumulation term, the momentum change term being calculated based on the joint angles and the joint angular velocities, the residual accumulation term being obtained by integrating the motor current, the current estimation residual, and a dynamic compensation term calculated based on a dynamic model, the dynamic compensation term being calculated using a specified dynamic algorithm based on the joint angles and the joint angular velocities; if the current estimation residual is not within a specified numerical range and the duration exceeds a specified duration, controlling the robot to stop moving through the task planning and execution module, and / or controlling the robot to maintain its current pose through the joint servo control module.

[0007] In some embodiments, when the Coriolis force and centrifugal force matrices are not calculated, the specified dynamics algorithm includes a difference calculation method based on the derivative of the inertia matrix; or, when the Coriolis force and centrifugal force matrices can be calculated, the specified dynamics algorithm includes an explicit calculation method based on a rigid body dynamics model.

[0008] In some embodiments, the collision detection module is provided with a control flag; the method further includes: when the current estimation residual is not within the specified numerical range and the duration exceeds the specified duration, providing the control flag to the task planning execution module and / or the joint servo control module; wherein the task planning execution module is used to control the robot to stop moving upon receiving the control flag, and / or the joint servo control module is used to control the robot to maintain its current pose upon receiving the control flag.

[0009] In some embodiments, the collision detection module further sets a possible collision flag and a confirmed collision flag, and the method further includes: when the current estimation residual is within the specified numerical range, setting the possible collision flag to a first flag value to indicate the possibility that no collision exists; or, when the current estimation residual is not within the specified numerical range, setting the possible collision flag to a second flag value to indicate the possibility that a collision exists; if the duration for which the current estimation residual is not within the specified numerical range does not exceed the specified duration, then setting the possible collision flag back to the first flag value and setting the confirmed collision flag to a third flag value to indicate that no collision is confirmed; or, if the duration for which the current estimation residual is not within the specified numerical range exceeds the specified duration, then setting the confirmed collision flag to a fourth flag value to indicate that a collision is confirmed and setting the control flag to a fifth flag value to indicate that collision control is required.

[0010] Secondly, embodiments of this application provide a collision detection module applied to a robot control device, the robot control device further including a task planning and execution module and / or a joint servo control module; the collision detection module includes: a data acquisition unit, used to acquire the robot's perception data, the perception data including joint angles, joint angular velocities, and motor currents; a residual estimation unit, used to input the perception data into an observer, and calculate a current estimation residual based on the observer; the observer includes a momentum change term and a residual accumulation term, the momentum change term is calculated based on the joint angles and joint angular velocities, the residual accumulation term is obtained by integrating the motor current, the current estimation residual, and a dynamic compensation term calculated based on a dynamic model, the dynamic compensation term is calculated using a specified dynamic algorithm based on the joint angles and joint angular velocities; a collision response unit, used to control the robot to stop moving through the task planning and execution module, and / or to control the robot to maintain its current pose through the joint servo control module, when the current estimation residual is not within a specified numerical range and the duration exceeds a specified duration.

[0011] Thirdly, embodiments of this application provide a robot control device, which includes a collision detection module, a task planning and execution module, and / or a joint servo control module; the collision detection module is used to execute the method described in any of the above-mentioned embodiments.

[0012] In some embodiments, the task planning and execution module is configured to, upon obtaining a new planned trajectory, control the robot to stop moving and cancel the execution of the new planned trajectory if the current estimation residual is not within a specified numerical range and the duration exceeds a specified duration.

[0013] In some embodiments, the joint servo control module is used to set the robot's current pose as the target pose and set the angular velocities of each joint of the robot to zero when the current estimation residual is not within a specified numerical range and the duration exceeds a specified duration, so that the robot maintains its current pose.

[0014] Fourthly, embodiments of this application provide a robot, which includes the robot control device described above.

[0015] Fifthly, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the method described in any of the preceding claims.

[0016] Sixthly, embodiments of this application provide a computer device, the computer device including a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the method described in any of the above-mentioned embodiments.

[0017] In a seventh aspect, embodiments of this application provide a computer program product, the computer program product including a computer program, which, when executed by a processor, implements the method described in any of the above-mentioned embodiments.

[0018] This application provides a collision detection method and module, a robot, and related devices. The method is applied to a collision detection module in a robot control device, which also includes a task planning and execution module and / or a joint servo control module. The method includes: acquiring sensory data of the robot, including joint angles, joint angular velocities, and motor currents; inputting the sensory data into an observer, and calculating a current estimation residual based on the observer; the observer includes a momentum change term and a residual accumulation term, the momentum change term being calculated based on joint angles and joint angular velocities, the residual accumulation term being obtained by integrating the motor current, the current estimation residual, and a dynamic compensation term calculated based on a dynamic model, and the dynamic compensation term being calculated using a specified dynamic algorithm based on joint angles and joint angular velocities; if the current estimation residual is not within a specified numerical range and its duration exceeds a specified duration, controlling the robot to stop moving via the task planning and execution module, and / or controlling the robot to maintain its current pose via the joint servo control module. Because this embodiment utilizes a robot dynamics model to calculate momentum change terms based on joint angles and angular velocities, and integrates and accumulates motor current, current estimation residuals, and dynamic compensation terms to obtain a residual accumulation term, a current estimation residual is formed based on the momentum change term and the residual accumulation term. The current estimation residual reflects the instantaneous deviation between the actual momentum change and the theoretical momentum change, thus enabling more accurate identification of abnormal momentum changes. When the current estimation residual is outside the specified value range and persists for more than the specified duration, a collision is determined to have occurred. Relevant robot control operations can then be executed promptly through the task planning and execution module and / or the joint servo control module, reducing further damage or safety hazards. In other words, this embodiment can effectively filter high-frequency noise and inertial disturbances during motion without relying on acceleration measurements, improving the sensitivity and stability of collision detection, achieving fast and accurate collision detection and response control, ensuring robot operational safety, and enhancing the robustness and reliability of collision detection. Attached Figure Description

[0019] The embodiments of this application are further described below with reference to the accompanying drawings and specific implementation details.

[0020] Figure 1This is a schematic flowchart of a collision detection method provided in an embodiment of this application.

[0021] Figure 2 This is a flowchart of a robot current loop upper limb collision detection and control execution based on a first-order momentum observer, provided in an embodiment of this application.

[0022] Figure 3 This is a structural block diagram of a collision detection module provided in an embodiment of this application.

[0023] Figure 4 This is a structural block diagram of a robot control device provided in an embodiment of this application.

[0024] Figure 5 This is a structural block diagram of a robot provided in an embodiment of this application.

[0025] Figure 6 This is a structural block diagram of a computer device provided in an embodiment of this application. Detailed Implementation

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

[0027] In the description of the embodiments of this application, it should be understood that the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, features defined with "first" and "second" may explicitly or implicitly include one or more of the stated features. In the description of the embodiments of this application, "multiple" means two or more, unless otherwise explicitly specified.

[0028] In relevant robot control methods or systems, collision detection, as a key technology for ensuring human-robot collaboration safety and equipment self-protection, is receiving increasing attention. Related collision detection methods typically rely on accelerometers or external force sensors. While these can achieve relatively accurate collision identification, their high cost of additional hardware, complex installation, and susceptibility to noise interference limit their widespread adoption and reliability in practical applications. Therefore, collision detection schemes based on the robot's own motor current signals and dynamic models are gradually becoming a research hotspot. By analyzing the robot's joint angles, joint angular velocities, and motor currents—all body-sensing data—and combining robot dynamics and momentum change theories, collision estimation and real-time monitoring are performed. However, most current collision detection methods based on dynamic models face two major challenges: firstly, directly comparing the difference between theoretical and actual currents is susceptible to high-frequency noise, leading to false alarms or missed alarms; secondly, many observation methods rely on acceleration signals, which are typically obtained by differentiating angular velocity, resulting in high noise and instability, severely impacting detection performance.

[0029] Therefore, this embodiment utilizes sensory data such as robot joint angles, joint angular velocities, and motor currents, and calculates the current estimation residual by integrating the residual accumulation term and momentum change term of the dynamic compensation term. This current estimation residual reflects the instantaneous difference between the actual torque and the theoretical torque of the robot's dynamics, effectively distinguishing collision signals from normal dynamic disturbances, thus improving the accuracy and robustness of collision detection. Furthermore, it mitigates the noise problem caused by the differential of the acceleration signal, and through the timing judgment of the residual, combined with specified threshold and duration parameters, accurately determines collision events. This allows the task planning and execution module and / or the joint servo control module to trigger the robot's response, ensuring the robot's safe operation in complex environments.

[0030] See Figure 1 , Figure 1 This is a schematic flowchart of a collision detection method provided in an embodiment of this application.

[0031] To improve related technologies, this application provides a collision detection method applied to a collision detection module in a robot control device. The robot control device also includes a task planning and execution module and / or a joint servo control module. See also... Figure 1 The method includes steps S101 to S103.

[0032] Step S101: Acquire the robot's perception data, which includes joint angles, joint angular velocities, and motor currents;

[0033] Step S102: Input the sensing data into the observer and calculate the current estimation residual based on the observer; the observer includes a momentum change term and a residual accumulation term. The momentum change term is calculated based on the joint angle and joint angular velocity. The residual accumulation term is obtained by integrating the motor current, the current estimation residual, and the dynamic compensation term calculated based on the dynamic model. The dynamic compensation term is calculated based on the joint angle and joint angular velocity using a specified dynamic algorithm.

[0034] Step S103: If the current estimation residual is not within the specified value range and the duration exceeds the specified duration, the robot is controlled to stop moving by the task planning and execution module, and / or the robot is controlled to maintain the current pose by the joint servo control module.

[0035] In some embodiments, perception data can refer to data collected by the robot through sensors. Perception data can be a multi-dimensional data set reflecting the robot's current state and / or information about its external environment. Perception data may include joint angles, joint angular velocities, and motor currents. Those skilled in the art can extend the perception data according to the specific needs of the scenario; for example, perception data may also include motor voltage, torque estimates, etc.

[0036] In some embodiments, joint angles can refer to the current position or posture information of the corresponding joint of the robot. Joint angles can reflect the degree of rotation or position of the corresponding joint of the robot. Joint angles are one of the basic descriptive parameters of the robot's current posture.

[0037] In some embodiments, joint angular velocity can refer to the rate of change of the corresponding joint angle of the robot, that is, the rotational speed of the corresponding joint per unit time. Joint angular velocity can reflect the dynamic information during the robot's motion.

[0038] In the above embodiments, joint angles refer, for example, to the generalized joint angles of the robot, and joint angular velocities refer, for example, to the generalized joint angular velocities of the robot. As an example, joint angular velocities can be obtained by differentiating the joint angles.

[0039] In some embodiments, motor current may refer to the current of the motor driving the corresponding joint of the robot. Motor current can be used to characterize the torque applied to the motor shaft. Motor current is one of the reflections of external torques (such as collision forces). Motor current is also the basis of measurement for the observer to calculate the current estimation residuals mentioned below.

[0040] Understandably, perception data can include all real-time measurement data and auxiliary information calculated from it that can be used to describe the robot's dynamic behavior, dynamic state, and interaction with the environment during operation. Perception data is a key input and feedback basis for achieving collision detection.

[0041] In some embodiments, an observer can refer to a virtual device or module that performs calculations based on a robot dynamics model and the aforementioned perception data. As an example, the core function of an observer could be to infer the difference between the actual dynamic state and the theoretically predicted state of the robot system by constructing and updating an estimation error (such as current estimation residual) variable, particularly for detecting external disturbances (such as collisions).

[0042] Understandably, the role of an observer can include indirectly estimating the external torque disturbances experienced by the robot from limited sensor data. In other words, an observer can act as a software "sensor" to observe physical quantities that are difficult for relevant hardware sensors to measure directly.

[0043] In some embodiments, the current estimation residual can be used to represent the difference between the theoretically calculated motor current and the actually measured motor current, given a robot dynamics model and corresponding sensing data. The current estimation residual can reflect the deviation between the dynamics model prediction and the actual action, especially the deviation caused by external disturbances (such as collision torque).

[0044] Understandably, current estimation residuals can serve as a sensitive indicator of external disturbances (such as collision torque). Dynamic updates to the current estimation residuals allow for the estimation and tracking of anomalous current components. This helps the observer integrate and filter abrupt changes or abnormal persistence in the current signal, enhancing its ability to identify genuine collisions. Furthermore, the changing trend of the current estimation residuals can distinguish between collision events and normal motion, reducing the false alarm rate.

[0045] In some embodiments, the momentum change term can be used to reflect the theoretical rate of momentum change of the corresponding joint of the robot under the dynamic model. The momentum change term can be used to predict the dynamic behavior under normal motion conditions, i.e., the ideal dynamic behavior of the robot under undisturbed conditions. The momentum change term can be calculated based on joint angles and joint angular velocities. The momentum change term can provide a benchmark for the calculation of current estimation residuals and error inference.

[0046] In some embodiments, the momentum change term may also refer to the change in the theoretical momentum of the corresponding joint of the robot.

[0047] Understandably, the momentum change term can serve as a reference benchmark in the observer's dynamic model, characterizing the theoretical rate of momentum change. The momentum change term reflects the invariance of the robot's structure and dynamics, providing a theoretical physical basis for comparison with actual motor currents. Furthermore, the momentum change term allows the observer to go beyond simply comparing current values; it enables more physically meaningful estimations of residual torque based on the dynamic model, facilitating the generation of sensitive offset signals for additional torques caused by collisions or external disturbances.

[0048] In some embodiments, the residual accumulation term can be used to accumulate the deviation between the actual motor current and the theoretical prediction. The residual accumulation term can reveal abnormal momentum change signals caused by external collisions or disturbances. The residual accumulation term can be the change over time by integrating the motor current, the current estimation residual, and the dynamic compensation term calculated based on the dynamic model, to enhance the identifiability of abnormal signals and help detect external collisions or disturbances. The residual accumulation term can refer to the cumulative amount obtained by integrating the motor current, the current estimation residual, and the dynamic compensation term calculated based on the robot dynamic model.

[0049] In some embodiments, the dynamic model can refer to a mathematical model describing the relationship between robot joint motion and forces. The dynamic compensation term can refer to the non-driven torque compensation extracted from the dynamic model. The dynamic compensation term is used to correct and compensate for the inherent dynamic effects within the robot. The dynamic compensation term may include Coriolis force and centrifugal force matrices, gravity terms, and friction. The dynamic compensation term is a key term for the observer to eliminate the robot's own dynamic influences and focus on external disturbance signals. The dynamic compensation term can be calculated using a specified dynamic algorithm based on joint angles and joint angular velocities.

[0050] Understandably, the current estimation residual in the momentum observer is the error term of the current estimation, reflecting the deviation between the current predicted based on the dynamic model and the actual measured current. To reduce the impact of high-frequency noise and obtain a smooth and reliable residual estimate, a cumulative effect, called the "residual accumulation term," can be formed by integrating the motor current, the current estimation residual, and the dynamic compensation term obtained based on the dynamic model over time. This accumulation of instantaneous errors through integration effectively filters noise and stabilizes the residual estimate, thereby enabling accurate judgment of the robot's collision state. In other words, the purpose of the residual accumulation term is to temporarily smooth out short-term noise and sudden disturbances, extracting the true abnormal signals. The residual accumulation term can filter and suppress high-frequency noise, reducing misjudgments caused by instantaneous fluctuations. Integrating and accumulating abrupt residual signals can enhance the sensitivity to detecting the persistence of collision events. Furthermore, it can make the detection results more stable, reducing false detections caused by short-term oscillations. Additionally, it can transform minute instantaneous errors into trackable dynamic variables, facilitating subsequent processing (such as collision alarms or control responses).

[0051] Understandably, dynamic models can accurately reflect the essential relationship between forces and motion at robot joints, serving as the theoretical foundation for current and dynamic inference. Through predictions made by dynamic models, the current or torque values ​​corresponding to the robot's ideal motion state can be calculated. The design of the observer relies on the dynamic model to distinguish between current changes caused by normal actuation and abnormal currents caused by external disturbances.

[0052] Understandably, dynamic compensation terms can be used to eliminate inherent dynamic disturbances, such as removing dynamic effects like inertia, moment of inertia changes, and gravity generated during normal robot motion from the measurement signal. By compensating for internal dynamics, false positives and false negatives can be reduced, improving the observer's sensitivity and reliability to external abnormal forces. It facilitates the transformation of complete dynamic equations into simpler expressions, particularly by summarizing complex multivariate torques into a single dynamic compensation term, simplifying integration and calculation. Furthermore, it can compensate for internal dynamic fluctuations, reducing errors caused by sensor noise and model mismatch, ensuring that the residual signal effectively reflects external collisions, and improving the observer's stability.

[0053] In some embodiments, the specified numerical range can refer to a threshold interval for the magnitude of the current estimation residual. The specified numerical range can be a pre-set upper and lower limit value. The specified numerical range can represent the allowable fluctuation range of the current estimation residual under normal conditions. For example, the upper limit of the specified numerical range can be 2 amperes, and the lower limit can be -2 amperes. If the current estimation residual is not within the specified numerical range, it means that the current estimation residual exceeds the normal error threshold range, indicating a significant deviation between the actual measured motor current and the model prediction. This suggests that the current estimation residual is likely an abnormal signal caused by an external collision or fault. In other words, threshold limiting is used to determine whether the current estimation residual is at an abnormal level, thereby filtering out the impact of minor, occasional error fluctuations on collision detection; when the current estimation residual exceeds the specified numerical range, it is considered that a collision or other abnormal situation may have occurred.

[0054] In some embodiments, the specified duration can refer to a time threshold during which the current estimation residual remains abnormal (exceeding a specified numerical range). The specified duration can be a set time window. For example, the specified duration can be 200 microseconds. If the duration exceeds the specified duration, it indicates that the abnormal signal is not a momentary fluctuation but is persistent, suggesting a high probability of collision or malfunction. In other words, by setting a specified duration, only when the duration of the abnormal signal exceeds the specified duration will it be considered a true collision signal, thereby balancing the sensitivity and stability of collision detection and reducing the response to occasional noise or mechanical vibration.

[0055] In some embodiments, if the current estimation residual is not within the specified value range and the duration exceeds the specified duration, the task planning module can take emergency measures immediately after receiving the abnormal signal. For example, it can control the robot to terminate the current motion task to reduce the possibility of more serious collisions or equipment damage caused by continued motion, and / or the joint servo module can keep the corresponding joint of the robot in the current position to reduce the danger caused by continued motion, while buying time for subsequent diagnosis and processing.

[0056] Understandably, by setting a "specified numerical range" and a "specified duration," it can be ensured that only truly abnormal signals trigger corresponding protective actions, reducing false shutdowns. For example, the task planning layer can issue a stop command, while the joint servo layer can stabilize the joint posture, ensuring the robotic arm stops safely and stably, guaranteeing human-machine safety, and effectively shielding against transient error interference, reducing frequent shutdowns caused by misjudgments that affect production efficiency. Those skilled in the art can determine the specific ranges of the "specified numerical range" and "specified duration" according to the actual needs of the scenario.

[0057] As can be seen, in some embodiments, the observer in this method can utilize the robot's dynamics model to calculate the momentum change term (theoretical momentum change) based on joint angles and angular velocities. This is then combined with the motor current, the real-time calculated current estimation residual, and the dynamic compensation term used to correct the theoretical momentum change to obtain a residual accumulation term. The current estimation residual is formed based on the momentum change term and the residual accumulation term. The current estimation residual reflects the deviation between the actual momentum change and the theoretical momentum change, i.e., the unexpected torque caused by external disturbances (such as collisions). Random noise can be filtered out by setting a threshold for the current estimation residual, achieving stable collision detection. Furthermore, the dependence on acceleration measurements can be eliminated, improving the observer's accuracy and robustness. For example, experiments have shown that when the robot steps, compared to directly comparing the difference between the theoretical and actual currents, the observer in this method can more effectively filter out the jitter caused by stepping. In other words, this method can achieve more accurate and robust detection of robot collisions, significantly reducing the probability of false alarms and false negatives. No acceleration signal is required, simplifying the system hardware requirements. By using dynamic integration and residual accumulation, high-frequency noise and non-collision vibration interference can be effectively suppressed, ensuring stable and reliable detection results. It can also trigger control responses in a timely manner, ensuring the safety of the robot and its surrounding environment.

[0058] In some embodiments, a first-order momentum observer can be designed based on a robot dynamics model. This first-order momentum observer can be used to detect external collision disturbances on the robotic arm, improving the accuracy and robustness of collision detection. The following is an introduction to the design principles or calculation process of this first-order momentum observer. First, the robot dynamics model satisfies the following expression:

[0059]

[0060] Where q represents the corresponding joint angle of the robot; This indicates the corresponding joint angular velocity of the robot; M(q) represents the angular acceleration of the corresponding joint of the robot; M(q) represents the inertia matrix of the corresponding joint of the robot. Let represent the Coriolis force and centrifugal force matrix; g(q) represents the gravity term, i.e., the gravitational torque acting on the corresponding joint; f represents the joint friction torque or joint friction force, i.e., the friction torque component acting on the joint; τ represents the control torque applied by the motor, i.e., the active input torque driving the robot's corresponding joint movement; τ ext This represents the external torque acting on the corresponding joint of the robot, such as the disturbance torque caused by an external collision. Assume the following linear mapping relationship exists between the motor current signal E and the torque τ:

[0061] E=K t τ (2);

[0062] Among them, K t This represents the conversion coefficient between the motor current E and the control torque τ applied by the motor. Then, combining the above dynamic model and the linear mapping relationship of the motor current, we can derive:

[0063]

[0064] Among them, E ext E represents the motor current caused by the corresponding external disturbance. ext =K t τ ext The expression for the momentum P of the corresponding joint of the robot can be:

[0065]

[0066] Here, P represents the theoretical momentum without observer correction, i.e., P is the theoretical value. Taking the derivative of the theoretical momentum P, we obtain the following expression:

[0067]

[0068] Substituting equation (4) into equation (3) and rearranging, we obtain the following expression:

[0069]

[0070] Since errors are inevitable in current prediction during practical applications, and the incompleteness of the dynamic model also leads to estimation residuals, the current estimation residual can be defined as the difference between the actual current and the model-predicted current, characterizing the estimation error and unmodeled dynamic effects in the system. Unmodeled dynamic effects refer to actual dynamic effects such as inertia, friction, and flexible deformation that are not accurately described or ignored when establishing the robot's dynamic model due to simplification assumptions or incomplete parameters. The current estimation residual can be used to represent the deviation between the predicted current and the actual current. The expression for the current estimation residual R can be: Where E can represent the actual motor current, This can represent the motor current predicted by the model. Therefore, the model for estimating the error using this momentum observer can be:

[0071]

[0072] in, This represents the estimated momentum or the rate of change of momentum calculated by the observer. In other words, in equation (5)... It refers to the rate of change of theoretical momentum, used in conjunction with... Through comparison and error calculation, a momentum observer is designed. The following is the dynamic update expression for the current estimation residual R in the observer:

[0073]

[0074] in, K represents the time derivative of the current estimation residual. o This represents a gain matrix or coefficient of the observer. o Used to adjust the amplitude and speed of observation error feedback. That is, K o This determines the observer's response speed and sensitivity to the current estimation residuals; for example, if K... o The larger K is, the faster the observer responds to the current estimation residuals, and the more quickly it can capture external force disturbances caused by collisions. However, if K o If K is too large, the observer will become overly sensitive to system noise, leading to more false alarms; if K... o If K is too small, the response will be slow, and minor collisions may be missed. In the observer's dynamic feedback loop, K... o Similar to an "amplifier" or "gain regulator," this ensures that the update of the current estimation residual is smooth and efficient. Then, equation (5) is substituted into equation (7) to eliminate the unknown external current term E. ext This leads to the following expression:

[0075]

[0076] Then, the time variable in equation (8) is integrated to obtain the expression for the current estimation residual:

[0077]

[0078] Where P represents the theoretical momentum of the corresponding joint of the robot at the current moment, P0 represents the theoretical momentum of the corresponding joint of the robot at the initial moment, and (P-P0) represents the change in theoretical momentum from the initial moment to the current moment. That is to say, the momentum change term can be K in equation (9). t (P-P0), the cumulative residual term can be derived from equation (9). Among them, the dynamic compensation term The observer can directly use equation (9) to calculate the current estimation residual.

[0079] In robot dynamics modeling, calculating the Coriolis force and centrifugal force matrices is complex and costly. Inaccurate calculations or the inability to obtain these matrices can negatively impact the real-time performance and accuracy of collision detection methods. To improve the real-time performance and accuracy of relevant collision detection methods, in some embodiments, when the Coriolis force and centrifugal force matrices cannot be calculated, the specified dynamics algorithm may include a differential calculation method based on the derivative of the inertia matrix; or, when the Coriolis force and centrifugal force matrices can be calculated, the specified dynamics algorithm may include an explicit calculation method based on a rigid body dynamics model.

[0080] In some embodiments, "the case where the Coriolis force and centrifugal force matrices are not calculated" can refer to situations where complex dynamic matrices cannot be solved in real time in robot control systems with limited computing power or simplified models. Instead, the derivative of the inertial matrix is ​​calculated using a time-difference approximation method to indirectly infer the motion dynamics. For example, situations where the Coriolis force and centrifugal force matrices are difficult to calculate.

[0081] It is understandable that the inertia matrix M(q) changes with the joint angle q, and its time derivative... It is very important in robot dynamics modeling, but directly calculating The calculation is usually complex or difficult to obtain a precise expression. Therefore, its derivative can be approximated by performing a difference calculation on the sampled values ​​of the inertia matrix M(q) at consecutive time points, that is, This difference approximation improves upon complex symbolic derivations and calculations, reduces computational complexity, and can be used for dynamic estimation when explicit representations of complete Coriolis and centrifugal forces are lacking. In other words, it can replace computationally intensive methods. This is because directly calculating the time derivative of the inertia matrix often involves the sign differentiation of multiple sets of parameters in the robot model, especially for complex structures such as multi-joint arms, which is time-consuming and prone to errors. The approximate derivative value obtained by difference can be substituted into the momentum observer, as shown in equation (9), to calculate the current estimation residual R, and then complete the dynamic estimation of collision detection, simplifying the calculation of the matrix derivative term in the dynamic observer. This method can be used in scenarios with limited computing power or lack of complete dynamic expression. In other words, the difference calculation method based on the derivative of the inertia matrix uses the inertia matrix data at continuous time points to make a simple difference quotient, estimate the derivative term, and reduce the complexity of dynamic modeling. When the Coriolis force and centrifugal force matrix When computation is difficult or the model is incomplete, a simplification method is used, employing the time derivative of the inertia matrix M(q). To estimate the change in system momentum, but without explicitly calculating it. Specifically, it is impossible to calculate In this case, the observer can directly use the above equation (9) to calculate the current estimation residual, where the derivative of the inertia matrix is... Using the finite difference method for approximate calculation, the entire observer design reduces the need for... This is due to the dependency. This typically involves the robot's inertial coupling, centrifugal force, and Coriolis terms, and the calculation process is complex and may lack a complete model or sufficient real-time computing resources. In other words, the Coriolis force and centrifugal force matrices are no longer calculated or estimated separately. It simplifies the calculation process, reduces the implementation difficulty, and improves the real-time performance of collision detection.

[0082] Understandably, the observer can directly use equation (9) to calculate the current estimation residual. That is, in the specific implementation of calculating the current estimation residual R, a difference calculation method based on the derivative of the inertia matrix is ​​used to replace the Coriolis force and centrifugal force matrices. The relevant contribution can be directly calculated using equation (9) to calculate the current estimation residual, without the need to introduce the Coriolis force and centrifugal force matrices. The calculations allow the observer to work in a simpler form.

[0083] In some embodiments, "being able to calculate the Coriolis force and centrifugal force matrices" can refer to a system possessing a complete robot rigid body dynamics model, capable of using a specialized dynamics library (such as Pinocchio) to calculate the Coriolis force and centrifugal force matrices in real time and accurately, and then estimating the torque for collision detection based on explicit dynamic terms. Pinocchio is an efficient open-source library for robot dynamics and kinematics calculations. Pinocchio can be used to quickly solve kinematic and dynamic matrices and equations related to robot systems. For example, using Pinocchio, accurate Coriolis force and centrifugal force matrices can be calculated online in real time, and the collision residuals can be explicitly calculated using the rigid body dynamics model to achieve high-precision collision detection. If the Coriolis force and centrifugal force matrices... If it can be calculated (e.g., using Pinocchio), then the accurate M(q) can be obtained by calling the aforementioned Pinocchio function.

[0084] g(q) and its related parameters, thereby effectively enhancing the accuracy and computational efficiency of the observer.

[0085] For example, when the Coriolis force and centrifugal force matrix When accurate calculation is possible, due to (10), therefore, we can substitute equation (10) into equation (9) to obtain the following expression:

[0086]

[0087] In other words, the momentum change term can be K in equation (11). t (P-P0), the cumulative residual term can be derived from equation (11) When K t When the matrix is ​​an identity matrix, equation (11) is essentially equivalent to an observer of the torque loop. That is, in this case, the observer is essentially a direct observation of the torque feedback, which simplifies the model.

[0088] For example, it can also make This leads to the following expression:

[0089]

[0090] in, This represents the dynamic compensation term. That is, the momentum change term can be (P-P0) or K from equation (13). t (P-P0), the cumulative residual term can be derived from equation (9). Or in equation (11)

[0091] Understandably, robot dynamics primarily employs two types of computational methods: explicit and implicit. Explicit computational methods based on rigid body dynamics models typically utilize the system's dynamic equations and physical parameters to directly calculate dynamic quantities such as joint torques and inertia matrices using explicit analytical formulas or recursive algorithms. For example, the Newton-Euler recursive algorithm calculates the inertial forces and torques at each joint recursively from bottom to top and then from top to bottom. This algorithm, based on rigid body dynamics equations, is efficient and has a clear form, belonging to explicit dynamic model solution methods. In contrast, implicit computational methods can include numerical iteration schemes (such as dynamics solutions based on implicit integration) or data-driven estimation methods. These methods do not explicitly rely on the analytical expressions of rigid body dynamics but rather estimate dynamic quantities based on relevant sensor data through numerical optimization, black-box models, or machine learning. These methods are suitable for complex systems or systems with incompletely accurate models.

[0092] It is understandable that when the inertia matrix M(q) and the Coriolis force and centrifugal force matrices are... When accurate calculation is possible, the observer can use the above-mentioned explicit calculation method based on the rigid body dynamics model, such as Equation (11) or Equation (13), to calculate the current estimation residual.

[0093] For example, taking a fixed-base six-DOF robotic arm as an example, the joint angle q(t) and joint angular velocity of the corresponding joints of the robotic arm can be collected in real time.

[0094] And the motor current E(t). Momentum is calculated using the robot's sensor data, such as... Using the Pinocchio dynamics library mentioned above, the inertia matrix M(q(t)), Coriolis force, and centrifugal force matrix are calculated in real time. The gravity vector g(q(t)) and friction force f are used. The observer can calculate the current estimation residual R(t) using equations (9), (11), or (13). Threshold judgments are made on the magnitude and duration of the current estimation residual R(t). If the threshold is exceeded, it is determined that there is external collision interference. Combined with the detection results, obstacle avoidance, deceleration, or other safety strategies of the robot can be triggered to ensure the safety of the robot's interaction with the environment.

[0095] As can be seen, in some embodiments, the differential calculation in this method, based on the approximation of the derivative of the inertia matrix, can quickly estimate the system's dynamic response when a complex moment model is lacking; while the explicit dynamic model, utilizing complete rigid body dynamics formulas, can provide more accurate dynamic information. The combination of these two approaches satisfies the need for a balance between computational accuracy and speed under different conditions. In other words, this method can effectively calculate dynamic parameters under varying computational resource and model acquisition conditions, ensuring that the collision detection algorithm possesses both good real-time performance and good computational accuracy and stability, thus improving the algorithm's applicability and flexibility.

[0096] As can be seen, the above embodiments do not rely on acceleration information, reducing noise interference from acceleration signals and improving stability. They can effectively filter out high-frequency noise, reducing false triggering caused by vibrations such as mechanical vibrations and robot stepping. Directly calculating the dynamic matrix using the Pinocchio dynamics library reduces errors from numerical difference calculations, improving the overall computational efficiency and observation accuracy of the system. Provided the dynamic model is accurate, the above embodiments exhibit high collision detection accuracy.

[0097] To achieve timely response of collision detection to robot actions, reduce mechanical damage or safety accidents caused by collisions, and ensure that the robot can safely and effectively stop or maintain its current position, in some embodiments, the collision detection module may be equipped with a control flag. The method may further include: providing the control flag to the task planning and execution module and / or the joint servo control module when the current estimation residual is not within a specified value range and persists for more than a specified duration; wherein the task planning and execution module controls the robot to stop moving upon receiving the control flag, and / or the joint servo control module controls the robot to maintain its current pose upon receiving the control flag.

[0098] In some embodiments, the control flag can refer to a logic signal or a status indicator variable. The control flag can be generated by the collision detection module. The control flag can be used to inform the task planning and execution module and / or the joint servo control module that "a collision anomaly has been detected, requiring emergency handling." The control flag can serve as a coordination interface signal between robot-related modules or subsystems (such as the task planning and execution module and the joint servo control module). When this method determines that an abnormal contact has occurred, the control flag is triggered, prompting the corresponding robot module or subsystem to perform a safety response and control action to prevent the robot from continuing to move after detecting a potential collision, thereby reducing safety accidents and mechanical damage.

[0099] It is understandable that the collision detection module has a control flag because the current estimation residual fluctuates instantaneously during robot operation. To reduce false alarms, the current estimation residual must continuously exceed a threshold for a duration exceeding a specified time before a collision is confirmed as real, and a control flag is generated. Alternatively, the control flag can be set to a specific flag value (e.g., the fifth flag value mentioned below). The control flag provides a standardized "collision warning" signal, which other modules or subsystems can use to respond promptly, achieving hardware and software collaborative safety protection, facilitating debugging and system integration, and improving the overall system robustness.

[0100] For example, suppose a robot's robotic arm is running on an assembly line. Its collision detection module calculates the current estimation residual and detects an anomaly, such as a current estimation residual exceeding a preset threshold (e.g., 20% of the rated current), and this anomaly lasts for more than 0.5 seconds to rule out occasional noise interference. Subsequently, a control flag is triggered. The collision detection module sends a "collision alarm" signal (as an example of a control flag) to the task planning and execution module and / or the joint servo control module. Upon receiving the control flag, the task planning and execution module can immediately pause subsequent path planning and motion generation to prevent the issuance of new motion commands. Upon receiving the control flag, the joint servo control module can maintain the current robot posture and stop driving the motors to move the joints to reduce the risk of more serious damage from a collision. Maintenance personnel can be promptly informed of the collision information for manual inspection or to trigger subsequent safety procedures.

[0101] As can be seen, in some embodiments, a control flag is set in the collision detection module. When the current estimation residual exceeds a preset range and the duration exceeds a set threshold, the control flag is triggered. After receiving the control flag, the task planning and execution module can stop the corresponding robot movement, and / or, after receiving the control flag, the joint servo control module can maintain the robot's current pose. Since collision events are determined by setting thresholds and durations, false alarms and the impact of instantaneous fluctuations can be reduced. Utilizing the control flag mechanism, collision detection can be decoupled from other related modules (such as the task planning and execution module and the joint servo control module) and communicate efficiently, ensuring accurate and timely response actions. In other words, this method can achieve rapid response to collision events, promptly stop robot movement or maintain the current pose, reduce the risk of damage to the robotic arm, and improve the safety and reliability of robot operation.

[0102] To more accurately distinguish whether a robot collision has occurred, reduce false positives or false negatives, and provide multi-level state judgments to support safety control and decision-making, in some embodiments, the collision detection module can also set a possible collision flag and a confirmed collision flag. The method may further include: setting the possible collision flag to a first flag value to indicate the possibility of no collision when the current estimation residual is within a specified numerical range; or setting the possible collision flag to a second flag value to indicate the possibility of a collision when the current estimation residual is not within the specified numerical range; if the duration for which the current estimation residual is not within the specified numerical range does not exceed a specified duration, then resetting the possible collision flag to the first flag value and setting the confirmed collision flag to a third flag value to indicate that no collision has been confirmed; or, if the duration for which the current estimation residual is not within the specified numerical range exceeds a specified duration, then setting the confirmed collision flag to a fourth flag value to indicate that a collision has been confirmed and setting a control flag to a fifth flag value to indicate that collision control is required.

[0103] In some embodiments, the "Possible Collision Flag" can be used to indicate the "probability" of a collision reflected by the current current estimation residual. The "Possible Collision Flag" is used to provide a preliminary judgment. The "Confirmed Collision Flag" can be used to indicate the confirmation status of a collision event, determining whether a collision has actually occurred. The "Confirmed Collision Flag" can be used to reduce false alarms.

[0104] In some embodiments, the "first flag value" can be used to indicate the possibility that no collision was detected, i.e., the current current estimation residual is within the normal range. In other words, the "first flag value" can indicate that the current judgment is that the situation is normal and no collision has occurred.

[0105] In some embodiments, the "second flag value" can be used to indicate the possibility of a collision, i.e., the current estimation residual is outside the normal range, indicating a possible collision. In other words, the "second flag value" can indicate that the current judgment is that a collision is suspected.

[0106] In some embodiments, the "third flag value" can be used to indicate an unconfirmed collision. That is, the "third flag value" can indicate that the current determination is that an anomaly exists but the duration has not exceeded the specified duration, and no corresponding response action is triggered.

[0107] In some embodiments, the "fourth flag value" can be used to indicate that a collision has been confirmed. That is, the "fourth flag value" can indicate that the current determination is that a real collision has been detected, and preparations are made to trigger the corresponding control.

[0108] In some embodiments, the "fifth flag value" can be used to indicate that collision control is triggered, i.e., an action (such as stopping movement or maintaining posture) is required.

[0109] Understandably, when the detected current estimation residual is within a specified range, the potential collision flag is set to the first flag value, indicating that the current collision detection module considers the probability of collision low, i.e., no collision. When the detected current estimation residual exceeds the specified range, the potential collision flag is set to the second flag value, indicating a potential collision and entering a warning state. If the duration for which the current estimation residual exceeds the specified range does not exceed a specified duration, it is considered that a collision has not yet been confirmed, so the potential collision flag is reset to the first flag value, and the confirmed collision flag is set to the third flag value, which is equivalent to "observation and waiting" for the next judgment. If the duration for which the current estimation residual exceeds the specified range exceeds the specified duration, the confirmed collision flag is set to the fourth flag value, and the control flag of the fifth flag value is triggered, i.e., a collision is recognized and immediate control is required (such as stopping the robot's movement).

[0110] It is understandable that setting multiple flags (such as the aforementioned potential collision flag, confirmed collision flag, and first flag value) serves to prevent non-continuous or sudden, sporadic abnormal residuals from immediately triggering a corresponding control response from the robot, i.e., hierarchical judgment, reducing false alarms. For example, a "potential collision" warning is given first to alert the system; only after confirmation has been completed and the warning period has elapsed, is emergency control executed, balancing sensitivity and stability. This allows relevant modules to implement different levels of response strategies based on the different states of "potential collision" and "confirmed collision." Those skilled in the art can determine the specific values ​​of the aforementioned first flag value, fifth flag value, control flags, etc., according to the actual needs of the scenario.

[0111] As can be seen, in some embodiments, the collision detection module of this method sets a "possible collision flag" and a "confirmed collision flag". When the current estimation residual is within the normal range, the "possible collision flag" is set to the first flag value. When the current estimation residual exceeds the specified range but the duration does not exceed the threshold, the "possible collision flag" is set to the second flag value, and the "confirmed collision flag" is set to the third flag value. When the current estimation residual exceeds the specified range and the duration exceeds the threshold, the "confirmed collision flag" is set to the fourth flag value, and the "control flag" can be activated simultaneously to initiate safety control measures. By jointly judging the specified numerical range of the current estimation residual and the time threshold condition, instantaneous abnormal interference is filtered out, and state management is achieved using flag variables, ensuring that collision recognition is both sensitive and robust, reducing false alarms or missed alarms. In other words, this method can achieve multi-level judgment and dynamic confirmation of collision state, significantly reducing false alarms or missed alarms caused by short-term fluctuations, improving the accuracy and reliability of collision detection, and providing a basis for the robot's safe response.

[0112] In some embodiments, higher-order or nonlinear momentum observers can be designed. For example, second-order or higher-order momentum information can be used, combined with acceleration estimation or other sensor data, to construct an expression for the current estimation residual. This enhances the observer's adaptability and robustness to complex robot dynamics and nonlinear disturbances (such as friction, flexible vibration, etc.), making collision detection more accurate and reliable. More complete robot dynamic models can also be established, including more detailed descriptions of joint flexibility and friction characteristics, to improve the model's realism and computational accuracy, providing a more accurate dynamic basis for the observer and control algorithms. Base attitude state estimation can also be introduced. By sensing and correcting changes in the robot base's attitude in real time, the directional influence of gravitational acceleration can be accurately corrected to address detection errors caused by base attitude changes, improving the accuracy of collision detection. Furthermore, by collecting a large amount of robot motor current, speed, and collision data under operating conditions, combined with manually labeled data, deep neural networks or reinforcement learning models can be trained. These models can then predict and form expressions for the current estimation residuals, achieving stronger generalization capabilities and intelligence. In other words, these improvements aim to enhance the performance and reliability of this method in complex real-world environments through more advanced mathematical models, real-time state estimation, and intelligent algorithms.

[0113] See Figure 2 , Figure 2 This is a flowchart of a robot current loop upper limb collision detection and control execution based on a first-order momentum observer, provided in an embodiment of this application.

[0114] See Figure 2 This flowchart illustrates the general process by which a robot detects collisions in real time using current estimation residuals and initiates protective control measures based on the collision detection results; among which, Figure 2 The task planning and execution module can be used to read the pre-planned robot motion trajectory and control the robot to complete the relevant task actions; the collision detection module can be used to continuously monitor the current estimation residual of the corresponding joint of the robot based on the first-order momentum observer to determine the collision state; the joint servo control module can be used to execute commands such as joint position and speed control to control the robot's actions and posture. The collision detection module can be set with a state flag array. The state flag array can contain possible collision flags (such as possible_collision), confirmed collision flags (such as confirmed_collision), and control flags (such as control_flags[4]) for state synchronization and event triggering. Specifically, the flowchart can be executed according to the following process.

[0115] See Figure 2 For the task planning and execution module, in the "start" phase, the task planning and execution module starts, reads the planned trajectory instructions, and determines whether there is a newly planned trajectory; if there is no newly planned trajectory, it reads the planned trajectory again; if there is a newly planned trajectory, it checks the control flags (such as control_flags[4]). When the task planning and execution module receives the control flags (such as the control flags can be 1), it executes. Figure 2 The command "Stop movement, abandon current trajectory" means that setting the speed command to zero will abandon the remaining trajectory and ensure the robot stops moving. If the task planning and execution module does not receive the control flag, then it will execute... Figure 2 The "execution trajectory" is read until the "movement ends". After that, the planned trajectory is read again.

[0116] See Figure 2For the collision detection module, in the "start" phase, the module begins calculating the current estimation residual and determines whether any current estimation residuals are outside the specified range. For example, the collision detection module can obtain sensing data such as the current joint angle, joint angular velocity, and motor current from the data sharing board, and use a first-order momentum observer to calculate the current estimation residual, storing the residual value on the data sharing board. If the current estimation residual is within the specified range, the possible collision flag is set to the first flag value (e.g., "false"), and the confirmed collision flag is set to the third flag value (e.g., "false"), while the control flag remains in a non-triggered state. If the current estimation residual exceeds the specified range, the possible collision flag is set to the second flag value (e.g., "true"), and it is determined whether the duration of the anomaly exceeds a preset specified duration. If the duration does not exceed the specified duration, the possible collision flag is reset to the first flag value (e.g., "false"), the confirmed collision flag is set to the third flag value (e.g., "false"), the control flag remains in a non-triggered state, and observation continues. If the duration exceeds the specified duration, the collision confirmation flag is set to the fourth flag value (e.g., "true"). Simultaneously, the control flag is set to the fifth flag value (e.g., "1"). After setting the collision confirmation flag to the fourth flag value (e.g., "true"), the "collision detection result" can be written to a specified field (e.g., the "collision_detected" field) within a specified data block (e.g., the "arm_collision_data" data block) in a data sharing board (e.g., data_board_). In other words, the "collision_detected" field in the "arm_collision_data" data block can be a variable specifically used to record the collision status of the robotic arm. The collision detection module publishes its confirmed collision information through this field, notifying other relevant modules that a collision event has occurred. The collision detection module can send the control flag to the task planning and execution module and / or the joint servo control module in real time. Setting the control flag to the fifth flag value (e.g., "1") indicates that a collision control action needs to be executed. Those skilled in the art can also determine the specific values ​​of the control flags according to actual needs. The aforementioned data sharing board can be used for data transfer between modules, and to cache collision detection data and control flags. The data sharing board can serve as a "data dashboard" or "database" for information sharing among related modules (such as the task planning and execution module, collision detection module, and joint servo control module), facilitating real-time reading and utilization of the latest servo data by these modules and ensuring data synchronization and collaborative work between them.

[0117] See Figure 2For the joint servo control module, in the "start" phase, the joint servo control module stores servo data (such as joint angle, joint angular velocity, motor current) to a shared data structure (such as a data sharing board, for example, databoard). The joint servo control module can detect control flags (such as control_flags[4]). If a control flag is detected, the robot's current pose remains unchanged (such as executing...). Figure 2 The command "Use the current position as the command, set the speed command to 0" prevents any additional joint movement, ensuring a safe state. If no control flag is detected, then execute... Figure 2 The phrase "retrieves data from the data sharing board and sends it down" means that the necessary data, such as collision detection results, the current state of each joint of the robot, and flag bits, are read from the data sharing board. "Sends down" indicates that the information obtained from the data sharing board is sent to the relevant modules, allowing them to adjust their actions or states based on this information. Afterwards, execution... Figure 2 The process begins with "Waiting for servo topic data." Afterward, the servo data is stored again in the shared data structure. "Waiting for servo topic data" indicates that the corresponding control program has entered a waiting state, awaiting the latest status information from the servo system, such as the position of a stopped joint, current feedback, etc. This step ensures that control decisions rely on the latest and most accurate execution feedback, reducing blind guessing or erroneous actions. In short, it first reads key status or collision detection information from the data sharing board and sends control commands to the robot to respond; then it enters a waiting state, waiting for robot servo feedback data to confirm the current status, verify that the action has been executed correctly, or determine whether a collision has occurred, in order to make the next decision.

[0118] Furthermore, a typical example of the robot's current loop upper limb collision detection and control execution process can be as follows: The robot starts from a standstill and executes a predetermined trajectory. The collision detection module continuously calculates the current estimation residual to monitor the robot's working status. When the current estimation residual exceeds a specified value range at a certain point in time, the potential collision flag is set to the second flag value. If the duration of the anomaly does not reach a threshold, the confirmed collision flag is set to the third flag value, the collision detection module is in an "observation and waiting" state, and no control is triggered. The relevant trajectory of the task planning execution module continues to execute. If the duration of the anomaly signal exceeds a specified duration, the confirmed collision flag is switched to the fourth flag value, and the control flag is set to the fifth flag value. Upon receiving the control flag, the task planning module can forcibly abandon the current trajectory, and the joint servo control module can maintain the current posture. The robot immediately enters a safe state. If the current estimation residual returns to normal and persists for a certain period of time, both the potential collision flag and the confirmed collision flag are reset, the control flag is canceled, the task planning execution module resumes normal operation, and new trajectory planning is allowed. Otherwise, the safe state is maintained to reduce potential harm. As can be seen, by setting "potential collision flags," "confirmed collision flags," and "control flags," combined with time-domain monitoring of current estimation residuals, multi-level judgment of the robot's collision state can be achieved, and motion planning execution and servo actions can be controlled accordingly. This design can effectively reduce frequent shutdowns caused by false alarms and respond quickly to continuous abnormal conditions, ensuring the safety of the robot and the environment.

[0119] The above describes the collision detection method. This application also provides a collision detection module applied to a robot control device. See also... Figure 3 , Figure 3 This is a structural block diagram of a collision detection module provided in an embodiment of this application. The robot control device also includes a task planning and execution module and / or a joint servo control module.

[0120] See Figure 3The collision detection module includes: a data acquisition unit 101, used to acquire the robot's perception data, including joint angles, joint angular velocities, and motor currents; a residual estimation unit 102, used to input the perception data into an observer and calculate the current estimation residual based on the observer; the observer includes a momentum change term and a residual accumulation term, the momentum change term is calculated based on joint angles and joint angular velocities, the residual accumulation term is obtained by integrating the motor current, the current estimation residual, and the dynamic compensation term calculated based on the dynamic model, and the dynamic compensation term is calculated based on joint angles and joint angular velocities using a specified dynamic algorithm; and a collision response unit 103, used to control the robot to stop moving through the task planning and execution module, and / or to control the robot to maintain its current pose through the joint servo control module, when the current estimation residual is not within a specified value range and the duration exceeds a specified duration.

[0121] In this embodiment, the specific functions and effects implemented by the collision detection module can be explained by referring to other embodiments of this application, and will not be repeated here.

[0122] The above describes the collision detection module. This application also provides a robot control device. The robot control device includes a collision detection module. The robot control device further includes a task planning and execution module and / or a joint servo control module. The collision detection module is used to execute the collision detection method of any of the above embodiments.

[0123] See Figure 4 , Figure 4 This is a structural block diagram of a robot control device provided in an embodiment of this application.

[0124] In some embodiments, see Figure 4 The robot control device may include a collision detection module 10, a task planning and execution module 20, and a joint servo control module 30.

[0125] In some embodiments, the task planning and execution module is used to control the robot to stop moving and cancel the execution of the new planned trajectory if the current estimation residual is not within the specified value range and the duration exceeds the specified duration when a new planned trajectory is obtained.

[0126] In some embodiments, the joint servo control module is used to set the robot's current pose to the target pose and set the angular velocities of each joint of the robot to zero when the current estimation residual is not within a specified numerical range and the duration exceeds a specified duration, so that the robot maintains its current pose.

[0127] In this embodiment, the specific functions and effects implemented by the robot control device can be explained by referring to other embodiments of this application, and will not be repeated here.

[0128] The above describes the robot control device. This application also provides a robot, which includes the robot control device described in any of the above embodiments. Those skilled in the art can determine the specific configuration of other modules of the robot according to the actual needs of the scenario.

[0129] See Figure 5 , Figure 5 This is a structural block diagram of a robot provided in an embodiment of this application.

[0130] In some embodiments, see Figure 5 The robot includes the robot control device 1 and the robotic arm 2 of any of the above embodiments.

[0131] The specific functions and effects achieved by the robot in this embodiment can be explained by referring to other embodiments of this application, and will not be repeated here.

[0132] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements any of the collision detection methods described above.

[0133] This application also provides a computer program product, which includes a computer program that, when executed by a processor, implements any of the collision detection methods described above.

[0134] The computer program product may be a portable compact disc read-only memory (CD-ROM) and include program code, and may run on a terminal device, such as a personal computer. However, the computer program product of the embodiments of this application is not limited thereto, and the computer program product may be any combination of one or more computer-readable media.

[0135] See Figure 6 , Figure 6 This is a structural block diagram of a computer device provided in an embodiment of this application.

[0136] This application also provides a computer device, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement any of the above-described collision detection methods.

[0137] The embodiments of this application do not limit the computer device, which may be, for example, a local computer device, a cloud computer device, a distributed computer device, etc.

[0138] The computer device may include: a memory 110, a processor 120, and a communication interface 130. The memory 110, the processor 120, and the communication interface 130 are connected through internal connection paths.

[0139] The memory 110 is used to store computer programs, which in some implementations may include code for implementing the methods of the embodiments of this application.

[0140] The processor 120 executes the computer program stored in the memory 110 to control the communication interface 130 to receive input data and information, and output operation results and other data. In some implementations, when the solutions of the embodiments of this application are implemented by software or firmware, the computer program used to implement the solutions of the embodiments of this application can be stored in the processor 120 and executed by the processor 120.

[0141] The memory 110 may be volatile memory or non-volatile memory, or may include both volatile and non-volatile memory. The non-volatile memory may be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. The volatile memory may be random access memory (RAM). It should be noted that the memory 110 described herein is intended to include, but is not limited to, any memory of these and other suitable types. As an example, the memory 110 includes random access memory (RAM), cache memory, and read-only memory (ROM). The memory 110 stores a computer program that can be executed by processor 120, causing processor 120 to implement the steps of any of the methods described above.

[0142] The processor 120 can be a central processing unit (CPU), or it can be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor, or the processor 120 can be any conventional processor.

[0143] In implementation, each step of the above method can be completed by the integrated logic circuitry of the hardware in the processor 120 or by instructions in software form. The method disclosed in the embodiments of this application can be directly implemented by the hardware processor, or by a combination of hardware and software modules in the processor 120. The software modules can be located in mature storage media in the art, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, or registers. This storage medium is located in the memory 110, and the processor 120 reads the information in the memory 110 and, in conjunction with its hardware, completes the steps of the above method. To avoid repetition, detailed descriptions are not provided here.

[0144] In some implementations, in addition to the hardware units described above, computer devices may also include software modules, such as operating systems, basic input / output systems (BIOS), and application software.

[0145] An operating system is used to manage the hardware and / or software resources of a computer device; it is the kernel and foundation of the computer. The operating system handles fundamental tasks such as managing and configuring memory, determining the priority of system resource allocation, controlling input and output devices, operating the network, and managing the file system. To facilitate user operation, most operating systems provide a user interface for interaction with the system.

[0146] The BIOS is used to perform hardware initialization during the power-on boot phase and to provide runtime services for the operating system and applications. In some implementations, the BIOS can also monitor and display processor temperature and execute temperature protection strategies.

[0147] Application software, also known as an application program, can be understood as software written for a specific user application purpose, and is one of the main categories of computer software. For example, application software can be a program used to achieve purposes such as power control and temperature management.

[0148] It is understood that the specific examples in this application are only intended to help those skilled in the art better understand the implementation of this application, and are not intended to limit the scope of protection of this application.

[0149] It is understood that in the various embodiments of this application, the sequence number of each process does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of this application.

[0150] It is understood that the various implementation methods described in this application can be implemented individually or in combination, and this application does not limit them.

[0151] Unless otherwise stated, all technical and scientific terms used in this application have the same meaning as commonly understood by one of ordinary skill in the art. The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to limit the scope of this application. The singular forms “a,” “the,” and “the” used in this application and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise.

[0152] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software 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.

[0153] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the embodiments described above can be referred to the corresponding processes in other embodiments, and will not be repeated here.

[0154] In the embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units 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 apparatuses or units may be electrical, mechanical, or other forms.

[0155] The units described 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 the technical solution in this application, depending on actual needs.

[0156] In addition, 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.

[0157] If a function is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, essentially, or the part that contributes to related technologies, or part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium 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 storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0158] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A collision detection method, characterized in that, A collision detection module is used in a robot control device, the collision detection module is equipped with a control flag, a possible collision flag and a confirmed collision flag; the robot control device also includes a task planning and execution module and / or a joint servo control module; The collision detection method includes: Acquire the robot's perception data, which includes joint angles, joint angular velocities, and motor currents; The sensed data is input into an observer, and the current estimation residual is calculated based on the observer. The observer includes a momentum change term and a residual accumulation term. The momentum change term is calculated based on the joint angle and the joint angular velocity. The residual accumulation term is obtained by integrating the motor current, the current estimation residual, and the dynamic compensation term calculated based on the dynamic model. The dynamic compensation term is calculated using a specified dynamic algorithm based on the joint angle and the joint angular velocity. If the current estimation residual is not within the specified value range and the duration exceeds the specified duration, the task planning and execution module controls the robot to stop moving, and / or the joint servo control module controls the robot to maintain the current pose. The collision detection method further includes: when the current estimation residual is not within the specified numerical range and the duration exceeds the specified duration, providing the control flag to the task planning execution module and / or the joint servo control module; The task planning and execution module is used to control the robot to stop moving when the control flag is received, and / or the joint servo control module is used to control the robot to maintain its current pose when the control flag is received; The collision detection method further includes: If the current estimation residual is within the specified numerical range, the possible collision flag is set to a first flag value to characterize the probability that no collision exists; or, If the current estimation residual is not within the specified numerical range, the possible collision flag is set to a second flag value to indicate the possibility of a collision; if the duration for which the current estimation residual is not within the specified numerical range does not exceed the specified duration, the possible collision flag is reset to the first flag value, and the confirmed collision flag is set to a third flag value to indicate that a collision has not been confirmed; or, if the duration for which the current estimation residual is not within the specified numerical range exceeds the specified duration, the confirmed collision flag is set to a fourth flag value to indicate that a collision has been confirmed, and the control flag is set to a fifth flag value to indicate that collision control is required.

2. The collision detection method according to claim 1, characterized in that, Without calculating the Coriolis force and centrifugal force matrices, the specified dynamics algorithm includes a difference calculation method based on the derivative of the inertia matrix; or, When the Coriolis force and centrifugal force matrices can be calculated, the specified dynamics algorithm includes an explicit calculation method based on a rigid body dynamics model.

3. A collision detection module, characterized in that, The collision detection module is used to implement the collision detection method described in claim 1 in a robot control device; the robot control device further includes a task planning and execution module and / or a joint servo control module. The collision detection module includes: A data acquisition unit is used to acquire the robot's perception data, which includes joint angles, joint angular velocities, and motor currents. A residual estimation unit is used to input the sensing data into an observer and calculate the current estimation residual based on the observer. The observer includes a momentum change term and a residual accumulation term. The momentum change term is calculated based on the joint angle and the joint angular velocity. The residual accumulation term is obtained by integrating the motor current, the current estimation residual, and the dynamic compensation term calculated based on the dynamic model. The dynamic compensation term is calculated using a specified dynamic algorithm based on the joint angle and the joint angular velocity. The collision response unit is used to control the robot to stop moving through the task planning and execution module and / or to control the robot to maintain its current pose through the joint servo control module when the current estimation residual is not within the specified value range and the duration exceeds the specified duration.

4. A robot control device, characterized in that, The robot control device includes a collision detection module, and the robot control device also includes a task planning and execution module and / or a joint servo control module; The collision detection module is used to execute the collision detection method according to any one of claims 1 to 2.

5. The robot control device according to claim 4, characterized in that, The task planning and execution module is used to control the robot to stop moving and cancel the execution of the new planned trajectory if the current estimation residual is not within the specified value range and the duration exceeds the specified duration when a new planned trajectory is obtained.

6. The robot control device according to claim 4, characterized in that, The joint servo control module is used to set the robot's current pose as the target pose and set the angular velocities of each joint of the robot to zero when the current estimation residual is not within the specified value range and the duration exceeds the specified duration, so that the robot maintains the current pose.

7. A robot, characterized in that, The robot includes the robot control device according to any one of claims 4 to 6.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the collision detection method according to any one of claims 1 to 2.

9. A computer device, characterized in that, The computer device includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements the collision detection method according to any one of claims 1 to 2.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the collision detection method according to any one of claims 1 to 2.

Images