Collision handling methods, devices, equipment and storage media for robotic arms

Abstract

This application relates to a collision handling method, apparatus, device, and storage medium for a robotic arm. It acquires multi-dimensional sensing data from the robotic arm and performs fusion filtering on the multi-dimensional sensing data to obtain multi-dimensional feature data. It periodically updates the dynamic activity thresholds for each dimension of the multi-dimensional feature data and performs multi-dimensional joint comparisons between the multi-dimensional feature data and the dynamic activity thresholds to output collision identifiers. Based on the collision identifiers and the multi-dimensional feature data, it determines the current working state of the robotic arm. When the current working state of the robotic arm is a collision retreat state, it generates a retreat command based on the multi-dimensional feature data to drive the robotic arm to perform a retreat motion. Compared with existing technologies, the technical solution of this application can effectively improve the accuracy and anti-interference capability of collision detection, minimizing interference with the task process while ensuring safety.

Description

Technical Field

[0001] This application relates to the technical field of robot control, and in particular to a collision handling method, apparatus, device, and storage medium for a robotic arm. Background Technology

[0002] With the rapid development of robotics technology, robotic arms are increasingly being used in industrial assembly, medical assistance, and home services. During the execution of tasks, robotic arms are prone to accidental collisions with the environment or the objects they are operating on. If these collisions are not detected and handled properly in a timely manner, they can easily lead to task interruption or equipment damage.

[0003] Existing collision handling methods typically use a single sensor signal combined with a fixed threshold for collision detection. For example, when the joint force exceeds a preset threshold, a collision is determined and an emergency stop is executed.

[0004] However, such methods have obvious shortcomings: on the one hand, single-dimensional sensor data is easily affected by noise, leading to false triggers or missed detections; on the other hand, fixed thresholds are difficult to adapt to the dynamic changes of different movement states of the robotic arm, which can easily cause false or missed collisions; in addition, existing collision handling methods are mostly to stop the machine immediately, which cannot minimize the impact on the task process while ensuring safety. Summary of the Invention

[0005] This application provides a collision handling method, apparatus, device, and storage medium for a robotic arm, which can effectively improve the accuracy and anti-interference of collision detection, and minimize the interference with the task process while ensuring safety.

[0006] In a first aspect, this application provides a collision handling method for a robotic arm, comprising: acquiring multi-dimensional sensing data of the robotic arm and performing fusion filtering on the multi-dimensional sensing data to obtain multi-dimensional feature data; periodically updating the dynamic activity thresholds corresponding to each dimension in the multi-dimensional feature data, and performing multi-dimensional joint comparison between the multi-dimensional feature data and the dynamic activity thresholds respectively to output a collision identifier; determining the current working state of the robotic arm based on the collision identifier and the multi-dimensional feature data; when the current working state of the robotic arm is a collision retreat state, generating a retreat command based on the multi-dimensional feature data to drive the robotic arm to perform a retreat movement.

[0007] In one possible implementation, acquiring the multi-dimensional sensing data of the robotic arm and performing fusion filtering on the multi-dimensional sensing data to obtain multi-dimensional feature data specifically includes: synchronously acquiring the multi-dimensional sensing data of the robotic arm at a preset high-frequency sampling period, wherein the multi-dimensional sensing data includes at least the joint force, joint feedback position, joint feedback speed, joint current, joint command position, and end-effector six-dimensional perturbation estimate of each joint in the robotic arm; adjusting the joint force based on a pre-stored static bias of the joint force, and adjusting the joint current based on a pre-stored static bias of the joint current, to obtain adjusted joint force and... Adjust the joint current; calculate the position deviation between the joint command position and the joint feedback position, perform weighted fusion of the adjusted joint force, the joint feedback velocity, and the position deviation to construct a fused observation value of the joint force, and perform Kalman filtering on the fused observation value of the joint force to output the calibrated joint force; perform Kalman filtering on the adjusted joint current and the estimated end-effector six-dimensional perturbation value respectively to output the calibrated joint current and the estimated end-effector six-dimensional perturbation value; use the calibrated joint force, the calibrated joint current, the estimated end-effector six-dimensional perturbation value, and the position deviation as the multi-dimensional feature data.

[0008] In one possible implementation, the periodic update of the dynamic activity threshold corresponding to each dimension in the multi-dimensional feature data specifically includes: maintaining a sliding window for each feature quantity corresponding to each dimension in the multi-dimensional feature data, wherein the sliding window is used to store historical feature quantities corresponding to a preset number of historical control cycles closest to the current control cycle, and in each control cycle, storing the current feature quantity calculated in the current control cycle into the corresponding sliding window to update the historical feature quantity in the sliding window; in each control cycle, calculating the statistical feature value corresponding to the historical feature quantity in the sliding window corresponding to each feature quantity, and calculating the candidate threshold corresponding to each feature quantity based on the statistical feature value; comparing the candidate threshold corresponding to each feature quantity with the preset lower limit threshold corresponding to the feature quantity, and determining the dynamic activity threshold of each feature quantity in the current cycle based on the comparison result.

[0009] In one possible implementation, the step of performing multi-dimensional joint comparison between the multi-dimensional feature data and the dynamic activity threshold to output a collision identifier specifically includes: comparing the value of the feature quantity of each dimension in the multi-dimensional feature data of the current control cycle with the dynamic activity threshold corresponding to the feature quantity; based on the comparison result, determining whether each dimension in the multi-dimensional feature data meets a preset candidate condition; counting the number of dimensions that meet the candidate condition in the current control cycle to obtain the total number of candidate dimensions; if the total number of candidate dimensions is not less than a preset minimum number of joint triggering dimensions, generating a first collision candidate identifier; otherwise, generating a second collision candidate identifier; determining whether the current control cycle is the cooling cycle of the robotic arm; if so, outputting a second collision confirmation identifier; otherwise, determining whether the number of control cycles that continuously generate the first collision candidate identifier has reached a preset number of de-jittering cycles; if so, outputting a first collision confirmation identifier; otherwise, outputting a second collision confirmation identifier; and using the first collision candidate identifier or the second collision candidate identifier, as well as the first collision confirmation identifier or the second collision confirmation identifier, as collision identifiers and outputting the collision identifiers.

[0010] In one possible implementation, the multi-dimensional feature data includes at least a joint force dimension, a six-dimensional end-effector perturbation estimation dimension, a joint current dimension, and a position deviation dimension. Specifically, the feature quantities of the joint force dimension include at least the maximum absolute value feature quantity of the joint force and the L2 norm feature quantity of the joint force; the feature quantities of the six-dimensional end-effector perturbation estimation dimension include at least the maximum absolute value feature quantity of the six-dimensional end-effector perturbation estimation and the L2 norm feature quantity of the six-dimensional end-effector perturbation estimation; the feature quantities of the joint current dimension include at least the maximum absolute value feature quantity of the joint current and the L2 norm feature quantity of the joint current; and the feature quantities of the position deviation dimension include at least the maximum absolute value feature quantity of the position deviation.

[0011] In one possible implementation, if the total number of candidate dimensions is not less than a preset minimum number of joint triggering dimensions, a first collision candidate identifier is generated; otherwise, after generating a second collision candidate identifier, the method further includes: determining whether the value of the maximum absolute value feature of the joint force in the joint force dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and a first preset strong triggering multiple; and determining whether the value of the maximum absolute value feature of the end-six-dimensional disturbance estimation value in the end-six-dimensional disturbance estimation value dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the end-six-dimensional disturbance estimation value dimension and a second preset strong triggering multiple; if the value of the maximum absolute value feature of the joint force exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and a first preset strong triggering multiple; If the value of the maximum absolute value feature of the six-dimensional disturbance estimate exceeds the product of the dynamic activity threshold corresponding to the six-dimensional disturbance estimate dimension and the second preset strong triggering multiple, then it is determined whether there is a second collision candidate identifier in the current collision identifier. If so, the second collision candidate identifier is modified to the first collision candidate identifier; otherwise, the current collision identifier is not modified. If the value of the maximum absolute value feature of the joint force does not exceed the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple, and the value of the maximum absolute value feature of the six-dimensional disturbance estimate does not exceed the product of the dynamic activity threshold corresponding to the six-dimensional disturbance estimate dimension and the second preset strong triggering multiple, then the current collision identifier is not modified.

[0012] In one possible implementation, the working states of the robotic arm include normal operation, warning, deceleration, collision retreat, and collision stop.

[0013] In one possible implementation, determining the current working state of the robotic arm based on the collision identifier and the multi-dimensional feature data specifically includes: when the collision identifier includes a first collision confirmation identifier, determining the current working state of the robotic arm as the collision retreat state; when the collision identifier includes a second collision confirmation identifier and a first collision candidate identifier, determining the current working state of the robotic arm as the deceleration operation state; when the collision identifier includes a second collision confirmation identifier and a second collision candidate identifier, and the value of the maximum absolute value of the joint force feature in the multi-dimensional feature data is greater than the product of a preset first threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, determining the current working state of the robotic arm as the warning state; when the current working state of the robotic arm is the warning state, and the maximum absolute value of the joint force... When the value of the feature quantity is less than the product of a preset second threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, the current working state of the robotic arm is switched back to the normal operating state, wherein the second threshold ratio is less than the first threshold ratio; when the current working state of the robotic arm is the deceleration operation state, and the number of control cycles for continuously generating the second collision candidate identifier reaches a preset number of continuous cycles, the current working state of the robotic arm is switched back to the normal operating state; when the current working state of the robotic arm is the collision retreat state, and a retreat completion flag or a retreat timeout flag is received, the current working state of the robotic arm is switched to the collision stop state; when the current working state of the robotic arm is the collision stop state, and an external recovery command is received, the current working state of the robotic arm is switched back to the normal operating state.

[0014] In one possible implementation, generating a retraction command based on the multi-dimensional feature data specifically includes: mapping the calibration joint forces in the multi-dimensional feature data to the Cartesian space of the robotic arm end effector based on the Jacobian matrix to obtain the collision force vector in the Cartesian space, and determining the retraction direction based on the collision force vector; generating a straight-line retraction trajectory in the Cartesian space based on the retraction direction and a preset retraction distance; determining the retraction speed according to a preset intensity range where the maximum absolute value feature of the joint forces in the multi-dimensional feature data is located; and calculating the retraction speed and a preset control cycle duration for each... The maximum permissible step value of the joint within a control cycle; in each control cycle, the linear retreat trajectory is discretized to obtain the desired end pose, and the desired end pose is solved based on inverse kinematics to obtain the desired joint angle; the joint angle increment between the desired joint angle and the joint feedback position of the current control cycle is calculated, and the joint angle increment is limited based on the maximum permissible step value to determine the limited joint angle increment; based on the joint angle increment and the joint feedback position of the current control cycle, the expected value of the joint position is determined, and the expected value of the joint position is output as a retreat command.

[0015] Secondly, this application provides a collision handling device for a robotic arm, comprising: a data processing module, a collision detection module, a state management module, and a retreat control module; wherein, the data processing module is used to acquire multi-dimensional sensing data of the robotic arm and perform fusion filtering processing on the multi-dimensional sensing data to obtain multi-dimensional feature data; the collision detection module is used to periodically update the dynamic activity thresholds corresponding to each dimension in the multi-dimensional feature data, and perform multi-dimensional joint comparison between the multi-dimensional feature data and the dynamic activity thresholds respectively, and output a collision identifier; the state management module is used to determine the current working state of the robotic arm based on the collision identifier and the multi-dimensional feature data; the retreat control module is used to generate a retreat command based on the multi-dimensional feature data when the current working state of the robotic arm is a collision retreat state, so as to drive the robotic arm to perform a retreat movement.

[0016] Thirdly, embodiments of this application also provide a computer device, which includes a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the above-described method.

[0017] Fourthly, embodiments of this application also provide a computer-readable storage medium storing a computer program that, when executed by a processor, can implement the above-described method. This application provides a collision handling method, apparatus, device, and storage medium for a robotic arm, which has the following advantages compared with the prior art: By acquiring multi-dimensional sensor data from the robotic arm and performing fusion filtering, noise interference susceptible to single sensors can be effectively suppressed, improving the accuracy of collision detection. Simultaneously, periodically updating the dynamic activity thresholds for each dimension and performing multi-dimensional joint comparisons can adaptively avoid false triggering or missed detections caused by fixed thresholds. Based on this, the current working state of the robotic arm is determined according to the collision marker and multi-dimensional feature data. When the robotic arm is in a collision retreat state, a retreat command is generated based on the multi-dimensional feature data to drive the robotic arm to perform active retreat motion, thus replacing the passive approach of immediate shutdown in traditional methods and minimizing interference with the task process while ensuring safety. Attached Figure Description

[0018] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0019] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] One or more embodiments are illustrated by way of example with reference numerals in the accompanying drawings. These illustrations do not constitute a limitation on the embodiments. Elements with the same reference numerals in the drawings are denoted as similar elements. Unless otherwise stated, the figures in the drawings are not to be limited by scale.

[0021] Figure 1 This is a flowchart illustrating one embodiment of a collision handling method for a robotic arm provided in this application; Figure 2 This is a schematic diagram of the structure of one embodiment of the collision handling device for a robotic arm provided in this application; Figure 3 This is a schematic diagram of the structure of a computer device provided in this application. Detailed Implementation

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

[0023] The following disclosure provides numerous different embodiments or examples for implementing various structures of this application. To simplify the disclosure, specific examples of components and arrangements are described below. These are merely examples and are not intended to limit the scope of this application. Furthermore, reference numerals and / or letters may be repeated in different examples. Such repetition is for simplification and clarity and does not in itself indicate a relationship between the various embodiments and / or arrangements discussed.

[0024] It should be understood that, when used in this specification and the appended claims, the terms "comprising" and "including" indicate the presence of the described features, integrals, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0025] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the scope of the application. As used in this specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0026] It should also be further understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0027] As used in this specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if [described condition or event] is detected" may be interpreted, depending on the context, as "once determined," "in response to determination," "once [described condition or event] is detected," or "in response to detection of [described condition or event]." Example 1, see Figure 1 , Figure 1 This is a flowchart illustrating one embodiment of a collision handling method for a robotic arm provided in this application. The collision handling method for a robotic arm described in this embodiment can be applied to the robotic arm of a single-arm robot, or separately to the left and right robotic arms of a dual-arm robot. When applied to a dual-arm robot, the left and right robotic arms are independent of each other, each executing the same collision detection and handling process, achieving decoupled control of the two arms. Figure 1 As shown, the method includes steps 101-104, as detailed below: Step 101: Obtain multi-dimensional sensing data of the robotic arm, and perform fusion filtering on the multi-dimensional sensing data to obtain multi-dimensional feature data.

[0028] In one embodiment, multi-dimensional sensing data of the robotic arm is synchronously collected at a preset high-frequency sampling period. The multi-dimensional sensing data includes at least the joint force, joint feedback position, joint feedback speed, joint current, joint command position, and end-effector six-dimensional perturbation estimate of each joint in the robotic arm.

[0029] Preferably, the preset high-frequency sampling period is 500Hz, that is, the single sampling interval is 0.002 seconds; the main controller uses the periodic timing signal of the high-frequency sampling period as the acquisition trigger condition, and synchronously starts multi-channel data reading in each control cycle to ensure that all sensor data are sampled at the same time, avoid timing errors caused by time-division acquisition, and ensure the data synchronization of high-frequency closed-loop control.

[0030] Specifically, the robotic arm consists of multiple joints; for example, in some embodiments, the robotic arm consists of seven joints.

[0031] Specifically, within each sampling period, the main controller calls the data reading function through the hardware interface to synchronously collect multi-dimensional sensor data from the robotic arm. This multi-dimensional sensor data includes joint forces, joint feedback positions, joint feedback speeds, joint currents, joint command positions, and the estimated six-dimensional perturbation values ​​at the robotic arm's end effector. The joint force of the j-th joint is denoted as... Joint forces are collected and output by force sensors independently configured for each joint, representing estimated external force disturbances. The joint feedback position of the j-th joint is denoted as... The joint feedback velocity is directly measured by the joint encoders of each joint and reflects the current actual angle of the joint. The joint current is calculated by numerical differentiation based on the position feedback from the joint encoder, reflecting the current actual movement velocity of the joint. The joint current of the j-th joint is denoted as... The current feedback signal read from the joint motor driver is used to characterize the real-time load condition of the motor. The joint command position is denoted as... This represents the desired position output by the trajectory planning or retreat control in the previous cycle. The estimated value of the final six-dimensional disturbance is denoted as... , where l The force is measured by a six-dimensional force sensor installed at the end of the robotic arm and mapped to Cartesian space via a Jacobian matrix. It is used to describe the contact force and torque between the end effector and the environment.

[0032] Specifically, in order to ensure the time alignment and synchronization of multi-dimensional sensor data, the main controller packages the sensor data of all the above dimensions into a raw data packet within the same sampling period for subsequent data processing, thereby meeting the requirements for real-time data and consistency in high-frequency motion control scenarios of robots.

[0033] In one embodiment, to eliminate the influence of the inherent bias of the sensor on collision detection, the main controller performs a baseline calibration after the robotic arm is powered on and in a static state without external force, in order to obtain the static bias of the joint force and the static bias of the joint current of each joint.

[0034] In some embodiments, before performing baseline calibration, the main controller first determines whether the robotic arm meets the baseline calibration conditions. The baseline calibration conditions include: all joints of the robot are stationary and the end effector is not subjected to any external contact force. When these conditions are met, the main controller initiates the static bias acquisition process. For the j-th joint, the main controller continuously acquires the raw values ​​of the joint force sensors and the joint current feedback values ​​for a preset number of cycles at the same sampling frequency as subsequent control cycles, such as 500Hz. The raw values ​​of the joint force sensors corresponding to the preset number of cycles are accumulated, and the sum of the accumulated raw values ​​of the joint force sensors is divided by the preset number of cycles to obtain the static bias of the joint force corresponding to the j-th joint. The joint current feedback values ​​corresponding to a preset number of cycles are accumulated. The sum of the accumulated joint current feedback values ​​is then divided by the preset number of cycles to obtain the static bias of the joint current corresponding to the j-th joint. Among them, the static offset of joint forces. This characterizes the fixed output of the joint force sensor at the j-th joint under zero external force conditions. This output is determined by a combination of factors, including gravitational torque, mechanical preload, and sensor zero drift. (Joint current static bias) This reflects the current noise floor of the motor driver of the j-th joint in the no-load static state.

[0035] Specifically, the main controller stores the calculated static bias of joint force and static bias of joint current for each joint in non-volatile memory, and calls it in each subsequent control cycle to subtract the corresponding bias from the real-time acquired raw value of joint force and feedback of joint current, thereby obtaining the adjusted signal that truly reflects the external force disturbance and load change.

[0036] It should be noted that baseline calibration is performed only once after the robot is powered on and the static conditions are met for the first time. If hardware is replaced, the ambient temperature changes significantly, or the sensor characteristics drift after long-term use, the operator can manually trigger recalibration via a host computer command to ensure the accuracy of the static bias.

[0037] In one embodiment, the joint force is adjusted based on a pre-stored static bias of the joint force, and the joint current is adjusted based on a pre-stored static bias of the joint current to obtain the adjusted joint force and the adjusted joint current; the position deviation between the joint command position and the joint feedback position is calculated, and the adjusted joint force, joint feedback velocity, and position deviation are weighted and fused to construct a fused observation value of the joint force, and the fused observation value of the joint force is processed by Kalman filtering to output the calibrated joint force; the adjusted joint current and the estimated value of the six-dimensional perturbation at the end point are processed by Kalman filtering respectively to output the calibrated joint current and the estimated value of the six-dimensional perturbation at the end point.

[0038] Specifically, for the j-th joint, in each control cycle, the main controller reads the joint force of the current control cycle. Joint current Joint feedback position Joint feedback speed and the joint command position output in the previous cycle ; to apply force to the joints Subtract the corresponding pre-stored joint force static bias To obtain adjusted joint force and joint current Subtract the corresponding pre-stored joint current static bias To obtain the adjusted joint current It can eliminate the fixed bias caused by factors such as gravitational torque, sensor zero drift, and preload, so that the characteristic signals of subsequent processing can truly reflect external force disturbances and load changes.

[0039] Specifically, the main controller will also control the joint command position. Subtract joint feedback position The positional deviation was obtained. The sign of the position deviation indicates whether the joint command is ahead or behind the feedback, and its absolute value reflects the position tracking error, serving as important auxiliary information for collision detection. The joint command position... This reflects the theoretically expected position of the j-th joint, the joint feedback position. This reflects the actual position reached by the j-th joint. When the position deviates... When positive, it indicates that the theoretically expected position of the j-th joint is greater than its actual position, which may lead to slow mechanical response. When the position deviation... When the value is negative, it indicates that the theoretically expected position of the j-th joint is less than its actual position, which may result in inertial displacement or other issues.

[0040] In some embodiments, multi-dimensional sensor data is fused and filtered in the following manner: by pre-setting velocity fusion weights. and positional deviation fusion weights Calculation speed fusion weight With joint feedback speed The first product, and the positional deviation fusion weights are calculated. with positional deviation The second product will adjust the joint forces. Adding the first and second products together yields the fused observations of the joint forces. The fused joint force observations integrate information from force, velocity, and position tracking error, providing a more accurate reflection of the collision contact state than raw values ​​from a single force sensor. The calculation formula for the fused joint force observations is as follows: .

[0041] Optional, speed fusion weights The value is 0.08, and the position deviation fusion weight is 0.15.

[0042] In some embodiments, the fused joint force observations are filtered to output calibrated joint forces. For example, in some embodiments, Kalman filtering is used to process the fused joint force observations. A one-dimensional Kalman filter is run independently for each joint to smooth the fused joint force observations constructed in the current control cycle and output calibrated joint forces. Specifically, the process of Kalman filtering the fused joint force observations for each joint is as follows: For the k-th control cycle, the main controller maintains two state variables for any j-th joint: the filtered joint force estimate from the previous control cycle (i.e., the (k-1)-th control cycle). And the estimation error covariance of the previous period Initially, the filter corresponding to the j-th joint is set to fuse the initial state of the joint forces with the observed values. As the initial joint force estimate Estimation error covariance The initial value is set to 1.0, that is: .

[0043] In each new control cycle, the filter first performs a state prediction step. During prediction, the filter assumes that the actual joint force in the current control cycle is the same as the filtered joint force estimate from the previous control cycle, i.e., the prior estimate of the joint force in the current cycle. The posterior estimate of the joint force from the previous cycle is directly taken. The posterior estimate of the joint force is the filtered joint force estimate from the previous control cycle, i.e. : Simultaneously, the joint force estimation error covariance from the previous cycle is also considered. Add the preset joint force process noise variance The covariance of the prior error of the joint force was obtained. ,Right now The noise variance in this process reflects the uncertainty of the system model itself. Optionally, the noise variance of the joint force process can be customized by those skilled in the art when implementing the embodiments of this application. For example, in some embodiments, .

[0044] Next, the filter calculates the Kalman gain of the joint force; this joint force Kalman gain The calculation method is as follows: the covariance of the prior error of joint forces is... Divide by the covariance of the prior error of joint force Compared with the preset joint force measurement noise variance The sum of these yields the Kalman gain of the joint forces. The joint force measurement noise variance represents the noise level measured by the sensor. The calculation formula is: Optionally, the joint force measurement noise variance can be customized by those skilled in the art when implementing the embodiments of this application. For example, in some embodiments, .

[0045] Then, the filter performs a state update. This is done by fusing the joint force observations from the current control cycle. Compared with prior estimates of joint forces Subtracting these values ​​yields the joint force information, which is the difference between the actual observation and the prediction. This joint force information is then multiplied by the Kalman gain of the joint force. This yields a correction value; this correction value is then added to the prior estimate of the joint force. The posterior estimate of the joint force for the current control cycle is obtained. That is, the filtered joint force corresponding to joint j Simultaneously, the covariance of the prior error of joint forces is used. A full ratio of 100% is represented as 1, using the Kalman gain of the joint force. This represents the proportion of observed corrections eliminated, calculated by subtracting the joint force Kalman gain from this 1. The first residual ratio is obtained, and then multiplied by the joint force prior error covariance. The posterior error covariance of the joint forces in the current control cycle is updated. This is used for calculations in the next cycle. The state update formula is as follows: ； 。

[0046] Through the recursive process of prediction, gain calculation, and state update described above, the filtered joint force corresponding to each joint j is output in each control cycle. Integrate the filtered joint forces corresponding to each joint j Calculate the joint force The calibrated joint force integrates current joint force, joint feedback velocity, and joint position deviation observation information, as well as historical estimation results, thereby effectively suppressing high-frequency noise and transient disturbances and outputting a smooth and reliable joint force signal.

[0047] Specifically, the main controller also performs one-dimensional Kalman filtering independently on the joint current of each joint to suppress high-frequency noise in the current signal and output a smooth and reliable calibrated joint current. The one-dimensional Kalman filtering process for adjusting the joint current is different from that for adjusting the joint force. The one-dimensional Kalman filtering for adjusting the joint current does not fuse the joint feedback velocity and position deviation to construct the fused observation value of the joint current, but directly uses the adjusted joint current as the fused observation value of the joint current for filtering.

[0048] Specifically, the process of performing Kalman filtering on the joint current corresponding to each joint is as follows: First, for the k-th control cycle, the main controller maintains two state variables for any j-th joint: the filtered joint current estimate from the previous cycle (i.e., the (k-1)-th control cycle). And the joint current estimation error covariance of the previous cycle. In the first control cycle ( The initial state of the filter is set to: the j-th adjustment joint current of the current cycle. As the initial joint current estimate Joint current estimation error covariance The initial value is set to 1.0, that is: ;in, for The adjustment joint current corresponding to the j-th joint in the process.

[0049] In each new control cycle, the filter first performs a state prediction step. During prediction, the filter assumes that the actual joint current of the current control cycle is the same as the filtered joint current estimate of the previous control cycle, i.e., the prior estimate of the joint current in the current cycle. The posterior estimate of the joint current from the previous control cycle is directly taken. The posterior estimate of the joint current is the filtered joint current estimate from the previous cycle. ,Right now Simultaneously, the joint current estimation error covariance of the previous cycle is also considered. Add the preset joint current process noise variance The prior error covariance of the joint current was obtained. ,Right now .

[0050] Preferably, the joint current process noise variance .

[0051] Next, the filter calculates the joint current Kalman gain. The joint current Kalman gain is calculated by taking the joint current prior error covariance... Divide by the prior error covariance of joint current Compared with the preset joint current measurement noise variance The sum of these yields the Kalman gain of the joint current. The calculation formula is: ;in, This represents the noise variance measured by the current sensor.

[0052] Then, the filter performs a state update. The joint current will be adjusted. Compared with the prior estimate of joint current Subtract the two to obtain the new information; multiply this new information by the Kalman gain of the joint current. The correction amount is then added to the prior estimate of the joint current. The posterior estimate of the joint current for the current cycle is obtained. That is, the filtered joint current corresponding to each joint j. Simultaneously, the covariance of the prior error of the joint current is used. The full ratio of 100% is represented as 1, using the Kalman gain of the joint current. This represents the proportion of observed corrections eliminated, calculated by subtracting the Kalman gain of the joint current from this 1. The second residual ratio is obtained, and then multiplied by the prior error covariance of the joint current. The posterior error covariance of the current control cycle is updated. The state update formula is as follows: ； 。

[0053] Through the above recursive process, the filtered joint current corresponding to joint j can be obtained. The integrated filter joint current corresponding to joint j. Obtain calibrated joint current This value has undergone Kalman smoothing, which effectively suppresses high-frequency noise and instantaneous fluctuations in the current feedback, providing a stable and reliable current characteristic signal for subsequent collision detection.

[0054] Specifically, the main controller also independently performs one-dimensional Kalman filtering on each dimension of the end six-dimensional disturbance estimate to suppress high-frequency noise in the end force / torque signal and output a smooth and reliable calibrated end six-dimensional disturbance estimate. In its Kalman filtering process, the end six-dimensional disturbance estimate is directly taken for one-dimensional Kalman filtering without deducting static bias or fusing speed or position deviation.

[0055] Specifically, the estimated six-dimensional perturbation at the end contains six components, corresponding to three force components and three torque components in Cartesian space, denoted as... Where l = 1, 2, ..., 6. The master controller maintains an independent one-dimensional Kalman filter for each component, and the filtering process for each component is exactly the same.

[0056] Specifically, taking the l-th component of the final six-dimensional perturbation estimate as an example, the filtering process of the final six-dimensional perturbation estimate is explained: First, the main controller maintains two state variables for the l-th component: the filtered six-dimensional disturbance estimate from the previous control cycle. And the covariance of the six-dimensional perturbation estimation error at the end of the previous cycle. In the first control cycle The initial state of the filter is set to the l-th component of the six-dimensional disturbance estimate at the end of the current control cycle. As an initial estimate, the initial value of the covariance of the terminal six-dimensional perturbation estimation error is set to 1.0, that is: .

[0057] In each new control cycle, the filter first performs a state prediction step. During prediction, the filter assumes that the actual terminal disturbance value of the current control cycle is the same as the filtering result of the previous cycle, i.e., the prior estimate of the six-dimensional terminal disturbance. Directly take the posterior estimate of the last six-dimensional perturbation of the previous period. ;Right now Simultaneously, the covariance of the last six-dimensional perturbation estimation error from the previous cycle is also included. Adding the preset end-point six-dimensional perturbation process noise variance The covariance of the prior error of the six-dimensional perturbation at the end was obtained. ,Right now .

[0058] Preferably, the noise variance of the terminal six-dimensional perturbation process .

[0059] Next, the filter calculates the Kalman gain of the final six-dimensional perturbation. The calculation method for the final six-dimensional perturbation Kalman gain is as follows: The covariance of the final six-dimensional perturbation prior error... Divide by the prior error covariance of the six-dimensional perturbation at the end The variance of the six-dimensional disturbance measurement noise at the end point is compared with the preset value. The sum; yields the terminal six-dimensional perturbation Kalman gain. The calculation formula is: ;in, This represents the noise variance measured by the end-effector six-dimensional force sensor.

[0060] Then, the filter performs a state update; it updates the original value of the six-dimensional disturbance estimate at the end of the current control cycle. , and the prior estimate of the six-dimensional perturbation at the end Subtracting these two values ​​yields the terminal six-dimensional perturbation innovation; this terminal six-dimensional perturbation innovation is then multiplied by the terminal six-dimensional perturbation Kalman gain. The correction for the final six-dimensional perturbation is obtained and then added to the prior estimate of the final six-dimensional perturbation. The above yields the posterior estimate of the final six-dimensional perturbation corresponding to the l-th component of the current period. That is, integrating the six-dimensional perturbation estimates at the filter end corresponding to all components. Simultaneously, the prior error covariance is used to perturb the terminal six-dimensional error. The full proportion of 100% is represented as 1, using the terminal six-dimensional perturbation Kalman gain. This represents the proportion of observed corrections eliminated, calculated by subtracting the Kalman gain of the final six-dimensional perturbation from this 1. The third residual ratio is obtained, and then multiplied by the covariance of the prior error of the final six-dimensional perturbation. The updated six-dimensional perturbation posterior error covariance of the current control cycle is obtained. This is used for calculations in the next cycle. The state update formula is as follows: ； 。

[0061] Specifically, the six-dimensional disturbance estimate at the filter end is integrated from all components of the output of each control cycle. Integrate the six-dimensional perturbation estimates at the filter end corresponding to all components. The six-dimensional perturbation estimate at the calibration end can then be obtained. The above filtering is performed independently and in parallel across six dimensions, outputting a calibrated six-dimensional end-effector perturbation estimate. This value is then smoothed using Kalman smoothing, effectively suppressing high-frequency noise and transient disturbances from the end-effector force sensor, providing stable and reliable Cartesian force and torque signals for subsequent collision detection.

[0062] In one embodiment, the calibration joint force, calibration joint current, calibration end-effector six-dimensional perturbation estimate, and position deviation are used as multi-dimensional feature data.

[0063] Specifically, after completing the static bias deduction, position deviation calculation and Kalman filtering, the main controller obtains four types of clean and reliable feature signals in each control cycle, and integrates them into a multi-dimensional feature data packet for subsequent collision detection and retreat control.

[0064] Step 102: Periodically update the dynamic activity threshold corresponding to each dimension in the multi-dimensional feature data, and perform multi-dimensional joint comparison between the multi-dimensional feature data and the dynamic activity threshold respectively, and output collision identifier.

[0065] In one embodiment, the multi-dimensional feature data includes at least the joint force dimension, the terminal six-dimensional perturbation estimation dimension, the joint current dimension, and the position deviation dimension. The feature quantities of the joint force dimension include at least the maximum absolute value feature quantity of the joint force and the L2 norm feature quantity of the joint force. The feature quantities of the terminal six-dimensional perturbation estimation dimension include at least the maximum absolute value feature quantity of the terminal six-dimensional perturbation estimation and the L2 norm feature quantity of the terminal six-dimensional perturbation estimation. The feature quantities of the joint current dimension include at least the maximum absolute value feature quantity of the joint current and the L2 norm feature quantity of the joint current. The feature quantities of the position deviation dimension include at least the maximum absolute value feature quantity of the position deviation.

[0066] Specifically, for the maximum absolute value feature of joint forces: the main controller takes the absolute value of the calibrated joint forces corresponding to each joint of the robotic arm, and then selects the maximum value from all the absolute values ​​of calibrated joint forces. This maximum value is the maximum absolute value feature of joint forces, which reflects the magnitude of the external force borne by the joint on the robotic arm that is most severely stressed at the current moment, and is used to quickly detect severe local collisions.

[0067] Specifically, for the L2 norm characteristic of joint forces: the main controller squares the calibrated joint forces of each joint of the robotic arm, then adds up the squared values ​​of all calibrated joint forces to obtain a sum of squared values, and then takes the square root of the sum of squared values. The result is the L2 norm characteristic of the joint forces. This characteristic reflects the overall level of the joint forces of the entire robotic arm and is more sensitive to contact forces distributed across multiple joints.

[0068] Specifically, regarding the maximum absolute value characteristic of the six-dimensional disturbance estimate at the end effector: the six-dimensional disturbance estimate at the end effector includes three force components and three torque components, for a total of six components. The main controller takes the absolute value of each of these six components and then finds the maximum value, which is the maximum absolute value characteristic of the six-dimensional disturbance estimate at the end effector. It reflects the maximum contact intensity experienced by the end effector in a certain direction in Cartesian space.

[0069] Specifically, for the L2 norm characteristic of the six-dimensional disturbance estimate at the end point: the master controller squares the values ​​of the six components and adds them together to obtain the sum of the squares of the components. Then, it takes the square root of the sum of the squares of the components, and the result is the L2 norm characteristic of the six-dimensional disturbance estimate at the end point. This characteristic reflects the comprehensive strength of the overall force and torque at the end point.

[0070] Specifically, for the maximum absolute value characteristic of joint current: after the main controller calibrates the joint current of each of the seven joints, it takes the absolute value of each joint current, and then finds the maximum value among these seven absolute values. This value is the maximum absolute value characteristic of joint current. This characteristic is used to detect abnormal surges in current in a single joint caused by a collision.

[0071] Specifically, for the L2 norm characteristic of the joint currents: the main controller squares the calibrated joint currents of each of the seven joints, sums the squared values ​​of all joint currents to obtain the sum of the squared joint currents, and then takes the square root of the sum of the squared joint currents. The result is the L2 norm characteristic of the joint currents. This characteristic reflects the change in the overall drive load and is particularly sensitive to collisions where multiple joints are subjected to forces simultaneously.

[0072] Specifically, for the maximum absolute value feature of position deviation: the main controller first takes the absolute value of the position deviation of each of the seven joints, and then finds the maximum value among them. This maximum value is the maximum absolute value feature of position deviation. This feature reflects the degree of deviation of the joint with the largest position tracking error among the joints of the robotic arm. When a collision occurs and position tracking is obstructed, this feature will increase significantly.

[0073] In one embodiment, periodically updating the dynamic activity threshold corresponding to each dimension in the multi-dimensional feature data specifically includes: maintaining a sliding window for each feature quantity corresponding to each dimension in the multi-dimensional feature data, wherein the sliding window is used to store the historical feature quantities corresponding to the most recent preset number of historical control cycles, and in each control cycle, storing the current feature quantity calculated in the current control cycle into the corresponding sliding window to update the historical feature quantities in the sliding window; in each control cycle, calculating the statistical feature value corresponding to the historical feature quantity in the sliding window corresponding to each feature quantity, and calculating the candidate threshold corresponding to each feature quantity based on the statistical feature value; comparing the candidate threshold corresponding to each feature quantity with the preset lower limit threshold corresponding to the feature quantity, and determining the dynamic activity threshold of each feature quantity in the current cycle based on the comparison result.

[0074] Specifically, for each feature, the main controller allocates a fixed-length sliding window in memory. For example, for the joint force dimension, a sliding window is maintained for the maximum absolute value of the joint force feature to store the historical maximum absolute value of the joint force corresponding to the most recent preset number of historical control cycles; and a sliding window is maintained for the L2 norm of the joint force feature to store the historical L2 norm of the joint force corresponding to the most recent preset number of historical control cycles. For the end-point six-dimensional disturbance estimation dimension, a sliding window is maintained for the maximum absolute value of the end-point six-dimensional disturbance estimation feature to store the historical maximum absolute value of the end-point six-dimensional disturbance estimation feature corresponding to the most recent preset number of historical control cycles; and a sliding window is maintained for the L2 norm of the end-point six-dimensional disturbance estimation feature. A sliding window for the L2 norm feature of the six-dimensional disturbance estimate at the end of the current control cycle is used to store the historical L2 norm feature of the six-dimensional disturbance estimate at the end of the current control cycle corresponding to a preset number of historical control cycles. For the joint current dimension, a sliding window for the maximum absolute value feature of the joint current is maintained to store the historical maximum absolute value feature of the joint current corresponding to a preset number of historical control cycles. A sliding window for the L2 norm feature of the joint current is also maintained to store the historical L2 norm feature of the joint current corresponding to a preset number of historical control cycles. For the position deviation dimension, a sliding window for the maximum absolute value feature of the position deviation is maintained to store the historical maximum absolute value feature of the position deviation corresponding to a preset number of historical control cycles.

[0075] Specifically, preferably, the preset number of historical control cycles is 1000 control cycles; in each control cycle, the main controller first stores the current feature value of the feature quantity calculated in the current cycle into the corresponding sliding window. When the window is full, the earliest historical feature value in the window is automatically removed, so as to keep the feature value sequence of the most recent 1000 cycles always stored in the window.

[0076] In one embodiment, the statistical characteristic value includes a first statistical characteristic value and a second statistical characteristic value.

[0077] Specifically, in each control cycle, after the sliding window is updated, the main controller calculates the statistical characteristic value of all historical characteristic values ​​within the sliding window corresponding to each characteristic quantity. In this embodiment, the statistical characteristic value includes a first statistical characteristic value and a second statistical characteristic value, wherein the first statistical characteristic value is the arithmetic mean of the historical characteristic values ​​within the sliding window, and the second statistical characteristic value is the standard deviation of the historical characteristic values ​​within the window. The calculation of the mean and standard deviation adopts conventional calculation formulas, and those skilled in the art can understand its specific implementation method, which will not be described in detail here.

[0078] In one embodiment, the candidate threshold corresponding to each feature is calculated based on statistical feature values. Specifically, this includes: adding the product of the first statistical feature value corresponding to each feature value and a preset multiple of the statistical feature value to the second statistical feature value corresponding to the feature value to obtain a sum; and calculating the product of the sum value and a preset safety coefficient to obtain the candidate threshold corresponding to each feature value.

[0079] Specifically, the main controller multiplies a preset statistical characteristic value multiple by the standard deviation of the characteristic quantity to obtain a product. This product is then added to the mean of the characteristic quantity to form a sum. It should be noted that different preset statistical characteristic value multiples can be configured for different characteristic quantities. For example, the multiple for the maximum absolute value of joint force can be set to 3.0, while the multiple for the L2 norm of joint force can be set to 2.5, to achieve differentiated adaptive adjustment for different physical quantities.

[0080] Specifically, the main controller multiplies the above sum by a preset safety factor to obtain the candidate threshold for the feature in the current control cycle. The safety factor is a constant greater than 1. Different features can be configured with different safety factors, and similarly, different features can be configured with the same safety factor. Preferably, the safety factor is 1.2, which is used to appropriately amplify the threshold, reserve a certain safety margin for the system, and prevent false triggering caused by normal signal fluctuations.

[0081] Specifically, through the above calculations, the main controller generates a candidate threshold for each feature that can reflect the dynamic characteristics of the current motion state. This candidate threshold retains the representativeness of historical statistical information and provides the necessary fault tolerance space through the safety factor, laying a good foundation for the final determination of the subsequent dynamic activity threshold.

[0082] Specifically, to prevent false collision detection triggers due to excessively low candidate thresholds caused by excessively small mean and standard deviation of historical feature values ​​within the sliding window when the robot is stationary or under extremely low load, this embodiment independently configures a preset lower threshold for each feature. This preset lower threshold is pre-calibrated based on the physical meaning of the feature and the actual characteristics of the robot body, and stored in the controller. Preferably, the preset lower threshold for the feature with the largest absolute value of joint force is set to 5.0, and the preset lower threshold for the feature with the L2 norm of joint force is set to 10.0; the preset lower threshold for the feature with the largest absolute value of the end effector six-dimensional perturbation estimate is set to 3.0, and the preset lower threshold for the feature with the L2 norm of the end effector six-dimensional perturbation estimate is set to 8.0; the preset lower threshold for the feature with the largest absolute value of joint current is set to 0.5 amperes, and the preset lower threshold for the feature with the L2 norm of joint current is set to 1.0 amperes; the preset lower threshold for the feature with the largest absolute value of position deviation is set to 0.02 radians. These preset lower thresholds ensure that even when the robot is completely stationary or moving in free space without any external disturbance, the dynamic activity threshold will not fall below a reasonable safety baseline.

[0083] Specifically, in each control cycle, the main controller compares the calculated candidate threshold for each feature with the corresponding preset lower limit threshold. If the candidate threshold is greater than or equal to the preset lower limit threshold, the main controller uses the candidate threshold as the dynamic activity threshold for that feature in the current cycle. Conversely, if the candidate threshold is less than the preset lower limit threshold, the main controller uses the preset lower limit threshold as the dynamic activity threshold. In other words, the larger of the candidate threshold and the preset lower limit threshold is selected as the dynamic activity threshold for that feature.

[0084] Specifically, the comparison and value determination process described above is performed independently for each feature quantity. That is, the feature quantity with the maximum absolute value of joint force is compared with its own candidate threshold and its own preset lower limit threshold. The feature quantity with the L2 norm of joint force is also processed independently. All features quantities under other dimensions are processed independently according to the same rules. Each feature quantity ultimately obtains a dynamic activity threshold for the current cycle. This threshold is then used to determine the candidate conditions for that feature quantity in the current cycle. By combining the adaptively calculated candidate threshold with the physical lower limit, the threshold can be dynamically adjusted to adapt to different motion conditions. At the same time, false triggering caused by excessively low thresholds due to statistical fluctuations is avoided, thus balancing the sensitivity and robustness of collision detection.

[0085] In one embodiment, multi-dimensional feature data is compared with dynamic activity thresholds in multiple dimensions to output collision identifiers. Specifically, this includes: comparing the value of each feature quantity in the multi-dimensional feature data of the current control cycle with the dynamic activity threshold corresponding to that feature quantity; determining whether each dimension in the multi-dimensional feature data meets preset candidate conditions based on the comparison results; counting the number of dimensions that meet the candidate conditions in the current control cycle to obtain the total number of candidate dimensions; generating a first collision candidate identifier if the total number of candidate dimensions is not less than a preset minimum number of joint triggering dimensions; generating a second collision candidate identifier otherwise; determining whether the current control cycle is the cooling cycle of the robotic arm; if so, outputting a second collision confirmation identifier otherwise; determining whether the number of control cycles that continuously generate the first collision candidate identifier has reached a preset number of de-jittering cycles; if so, outputting a first collision confirmation identifier otherwise; and using the first collision candidate identifier or the second collision candidate identifier, as well as the first collision confirmation identifier or the second collision confirmation identifier, as collision identifiers and outputting the collision identifiers.

[0086] In one embodiment, if the total number of candidate dimensions is not less than a preset minimum number of joint triggering dimensions, a first collision candidate identifier is generated; otherwise, after generating a second collision candidate identifier, the method further includes: determining whether the value of the maximum absolute value feature of the joint force dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and a first preset strong triggering multiple; and determining whether the value of the maximum absolute value feature of the end-six-dimensional disturbance estimation value in the end-six-dimensional disturbance estimation value dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the end-six-dimensional disturbance estimation value dimension and a second preset strong triggering multiple; if the value of the maximum absolute value feature of the joint force exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple... If the product of multiples or the value of the maximum absolute value feature of the terminal six-dimensional perturbation estimate exceeds the product of the dynamic activity threshold corresponding to the terminal six-dimensional perturbation estimate dimension and the second preset strong triggering multiple, then it is determined whether there is a second collision candidate identifier in the current collision identifier. If so, the second collision candidate identifier is modified to the first collision candidate identifier; otherwise, the current collision identifier is not modified. If the value of the maximum absolute value feature of the joint force does not exceed the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple, and the value of the maximum absolute value feature of the terminal six-dimensional perturbation estimate does not exceed the product of the dynamic activity threshold corresponding to the terminal six-dimensional perturbation estimate dimension and the second preset strong triggering multiple, then the current collision identifier is not modified.

[0087] Specifically, for each feature in each dimension, it is determined whether the value of the feature is greater than or equal to the corresponding dynamic activity threshold; if any feature in any dimension satisfies this condition, then the dimension is determined to meet the preset candidate condition. For example, for the joint force dimension, it is determined whether the maximum absolute value of the joint force characteristic reaches or exceeds its corresponding dynamic activity threshold, or whether the L2 norm characteristic of the joint force reaches or exceeds its corresponding dynamic activity threshold. If so, the joint force dimension is marked as a candidate dimension. For the terminal six-dimensional perturbation estimation dimension, it is determined whether the maximum absolute value of the terminal six-dimensional perturbation estimation value reaches or exceeds its corresponding dynamic activity threshold, or whether the L2 norm characteristic of the terminal six-dimensional perturbation estimation value reaches or exceeds its corresponding dynamic activity threshold. If so, the terminal six-dimensional perturbation estimation dimension is marked as a candidate dimension. For the joint current dimension, it is determined whether the maximum absolute value of the joint current characteristic reaches or exceeds its corresponding dynamic activity threshold, or whether the L2 norm characteristic of the joint current reaches or exceeds its corresponding dynamic activity threshold. If so, the joint current dimension is marked as a candidate dimension. For the position deviation dimension, it is determined whether the maximum absolute value of the position deviation characteristic reaches or exceeds its corresponding dynamic activity threshold. If so, the position deviation dimension is marked as a candidate dimension.

[0088] Specifically, after determining the candidate conditions for all dimensions, the main controller counts the number of dimensions that meet the candidate conditions in the current control cycle, denoted as the total number of candidate dimensions, representing the number of dimensions that simultaneously trigger the candidate conditions. Subsequently, the main controller compares the total number of candidate dimensions with a preset minimum number of jointly triggered dimensions. If the total number of candidate dimensions is greater than or equal to the minimum number of jointly triggered dimensions, a first collision candidate identifier is generated, indicating that multiple dimensions simultaneously meet the candidate conditions in the current control cycle, and there is a high probability of a real collision occurring; otherwise, a second collision candidate identifier is generated, indicating that the number of candidate dimensions is insufficient, and the event may be noise or a non-collision event.

[0089] Preferably, the minimum number of joint triggering dimensions is set to 2.

[0090] Specifically, after generating the first or second collision candidate identifier by determining the total number of candidate dimensions, the main controller further executes a strong trigger fallback judgment to prevent missed detections due to extreme and severe collision scenarios not covered by the multi-dimensional joint triggering conditions; this strong trigger fallback logic is independent of the aforementioned total number of candidate dimensions and serves as a safety redundancy barrier.

[0091] Specifically, the main controller first determines whether two key physical quantities in the current control cycle exceed preset multiples of their respective dynamic activity thresholds. The first key physical quantity is the joint force: it calculates whether the value of the maximum absolute value characteristic of the joint force is greater than or equal to the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple. The second key physical quantity is the estimated value of the six-dimensional disturbance at the end point: it calculates whether the value of the maximum absolute value characteristic of the estimated value of the six-dimensional disturbance at the end point is greater than or equal to the product of the dynamic activity threshold corresponding to the estimated value of the six-dimensional disturbance at the end point and the second preset strong triggering multiple.

[0092] Preferably, the first preset strong trigger multiple and the second preset strong trigger multiple can be set to the same value, such as 2.5 times, but they can also be configured independently according to their respective physical characteristics.

[0093] Specifically, if the maximum absolute value of the joint force exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple, or if the maximum absolute value of the end-point six-dimensional disturbance estimate exceeds the product of the dynamic activity threshold corresponding to the end-point six-dimensional disturbance estimate dimension and the second preset strong triggering multiple, the main controller determines that there is a serious collision risk in the current control cycle and needs to forcibly set the current collision candidate identifier to the first collision candidate identifier. At this point, the main controller checks the collision candidate identifiers generated in the current control cycle: if the current collision candidate identifier is the second collision candidate identifier, it is modified to the first collision candidate identifier; if the current collision candidate identifier is already the first collision candidate identifier, no modification is needed, and the original value remains unchanged; this ensures that the final collision candidate identifier under strong triggering conditions is always the first collision candidate identifier; conversely, if the maximum absolute value feature of the joint force does not exceed the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple, and the maximum absolute value feature of the end-point six-dimensional disturbance estimation value does not exceed the product of the dynamic activity threshold corresponding to the end-point six-dimensional disturbance estimation value dimension and the second preset strong triggering multiple, then the main controller does not make any modification to the generated collision candidate identifiers, keeping the original result obtained based on the total number of candidate dimensions unchanged.

[0094] Specifically, through the aforementioned strong trigger fallback correction logic, an independent single-dimensional hard trigger channel is provided for severe collisions on the basis of conventional multi-dimensional joint triggering. When the collision intensity is so high that other dimensions have not yet responded, the system can still reliably generate the first collision candidate identifier, thereby ensuring the zero-miss detection security of collision detection, while not affecting the noise suppression effect of multi-dimensional joint triggering under normal operating conditions.

[0095] Specifically, to avoid repeated triggering within a short period after collision confirmation, and to prevent false confirmations caused by signal jitter, the main controller further performs cooling and debouncing checks. The main controller maintains a cooling timer to indicate whether the current control cycle is in a cooling cycle. If the cooling timer is greater than zero, it indicates that the current control cycle is a cooling cycle, and the main controller directly outputs the second collision confirmation flag without performing subsequent debouncing checks, and decrements the cooling timer by 1.

[0096] Specifically, if the current control cycle is not a cooling cycle (i.e., the cooling timer is zero), the main controller checks the number of control cycles that continuously generate the first collision candidate identifier. The main controller maintains a continuous count variable to record the number of cycles that continuously satisfy the first collision candidate identifier. When the collision candidate identifier of the current cycle is the first collision candidate identifier, the continuous count is incremented by 1; otherwise, the continuous count is cleared. Then, it is determined whether the continuous count has reached the preset number of debouncing cycles. In this embodiment, the preset number of debouncing cycles is set to 6 control cycles. If the continuous count reaches or exceeds the preset number of debouncing cycles, the first collision confirmation identifier is output, indicating that the collision has been reliably confirmed. At the same time, the cooling timer is set to one preset value cycle, and the continuous count is cleared. If the continuous count has not reached the number of debouncing cycles, the second collision confirmation identifier is output.

[0097] Preferably, the preset number of de-shaking cycles is set to 6 control cycles; the preset value cycle is 150 cycles.

[0098] Step 103: Determine the current working state of the robotic arm based on the collision identifier and the multi-dimensional feature data.

[0099] In one embodiment, the main controller determines the current working state of the robotic arm in each control cycle based on the obtained collision identifiers, including collision candidate identifiers and collision confirmation identifiers, as well as multi-dimensional feature data, and in accordance with preset state switching rules.

[0100] In one embodiment, the working states of the robotic arm include normal operation state, warning state, deceleration operation state, collision retreat state, and collision stop state.

[0101] Specifically, the normal operating state is the default working state of the robotic arm when there is no risk of collision. In this state, the main controller continuously performs collision detection and normal trajectory tracking, and issues normal motion commands to the joints.

[0102] Specifically, the warning status indicates that the joint force signal is close to the adaptive threshold but has not yet met the collision candidate conditions. At this time, the main controller only outputs the alarm log and does not change the motion command to remind the staff to pay attention to the equipment status.

[0103] Specifically, the deceleration operation state indicates that the collision candidate conditions have been met but the continuous de-jittering cycle has not yet been reached, meaning that the collision has not yet been definitively confirmed. In this state, the main controller reduces the joint speed limit and acceleration limit of the arm, for example, limiting the speed to 50% of the normal value, in order to converge the motion command earlier and reduce the risk of collision impact.

[0104] Specifically, the collision retreat state is the state that the main controller enters immediately after confirming a collision. In this state, the main controller activates the graded retreat control module, generates a retreat command based on the direction and intensity of the collision force, and drives the robot to perform a retreat motion in the opposite direction of the collision.

[0105] Specifically, the collision stop state indicates that after retracting to the correct position or after a timeout, the robot has maintained its current safe position and will no longer move actively. In this state, the main controller controls the joints to maintain their current positions and no longer issues movement commands, waiting for manual or automatic recovery.

[0106] In one embodiment, when the collision identifier includes a first collision confirmation identifier, the current working state of the robotic arm is determined to be a collision retreat state.

[0107] Specifically, when the collision flag includes a first collision confirmation flag, meaning the collision has been confirmed by debouncing, the main controller immediately determines the robot arm's current operating state as a collision retreat state, regardless of other conditions. This collision retreat state has the highest priority; once a collision is confirmed, the system prioritizes executing the retreat protection action.

[0108] In one embodiment, when the collision identifier includes a second collision confirmation identifier and a first collision candidate identifier, the current working state of the robotic arm is determined to be a deceleration operation state.

[0109] Specifically, when the collision flag does not contain the first collision confirmation flag (meaning the collision has not yet been confirmed), but contains both the second collision confirmation flag and the first collision candidate flag (meaning the candidate condition has been met but debouncing confirmation has not yet been completed), the main controller determines the current operating state of the robotic arm as a deceleration operation state. In this state, the main controller reduces the joint movement speed and acceleration limit to converge motion commands earlier and reduce potential collision impacts.

[0110] In one embodiment, when the collision identifier includes a second collision confirmation identifier and a second collision candidate identifier, and the value of the maximum absolute value feature of the joint force in the multi-dimensional feature data is greater than the product of the preset first threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, the current working state of the robotic arm is determined to be a warning state.

[0111] Specifically, when the collision identifier includes a second collision confirmation identifier and a second collision candidate identifier, that is, the candidate dimension is insufficient and the collision is not confirmed, and the value of the maximum absolute value feature of the joint force in the multi-dimensional feature data is greater than the product of the preset first threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, the main controller determines the current working state of the robotic arm as a warning state.

[0112] Preferably, the first threshold ratio can be, for example, 0.8, which means that the joint force has reached 80% of the corresponding dynamic activity threshold, but has not yet met the candidate conditions. At this time, the system only outputs an alarm log and does not change the motion command, reminding the operator to pay attention to the equipment status.

[0113] In one embodiment, when the current working state of the robotic arm is a warning state, and the value of the maximum absolute value of the joint force is less than the product of a preset second threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, the current working state of the robotic arm is switched back to the normal operating state, wherein the second threshold ratio is less than the first threshold ratio.

[0114] Specifically, if the current working state is a warning state, the main controller checks in each control cycle whether the value of the maximum absolute value of the joint force characteristic is less than the product of the preset second threshold ratio and the dynamic activity threshold corresponding to the joint force dimension.

[0115] Preferably, the second threshold ratio is less than the first threshold ratio, which can be 0.6. When the maximum absolute value of the joint force is less than this product, it indicates that the external force has moved away from the threshold, the risk is eliminated, and the main controller switches the working state of the robotic arm back to the normal operating state.

[0116] In one embodiment, when the current working state of the robotic arm is a deceleration operation state, and the number of control cycles for continuously generating the second collision candidate identifier reaches a preset number of continuous cycles, the current working state of the robotic arm is switched back to the normal operation state.

[0117] Specifically, if the current operating state is a deceleration operation state, the main controller detects the number of control cycles in which the second collision candidate identifier is continuously generated. When this number of consecutive cycles reaches a preset number of duration cycles, it indicates that the collision candidate state has stably disappeared, and the main controller switches the robot arm's operating state back to normal operation.

[0118] Preferably, the preset number of durations is 10 cycles.

[0119] In one embodiment, when the current working state of the robotic arm is the collision retreat state, and a retreat completion flag or a retreat timeout flag is received, the current working state of the robotic arm is switched to the collision stop state.

[0120] Specifically, if the current working state is collision retreat, the main controller will detect in real time whether it receives a retreat completion flag or a retreat timeout flag. When the retreat control module completes the retreat movement or the retreat time exceeds the preset timeout duration (e.g., 1.2 seconds), the main controller will switch the working state of the robotic arm to collision stop state.

[0121] In one embodiment, when the current working state of the robotic arm is the collision-stopped state and an external recovery command is received, the current working state of the robotic arm is switched back to the normal operating state.

[0122] Specifically, if the current operating state is a collision-stopped state, the main controller waits for an external recovery command, such as a reset signal sent from the host computer. Upon receiving a valid external recovery command, the main controller switches the robotic arm's operating state back to normal operation and restarts normal trajectory tracking and collision detection.

[0123] By combining multiple conditions based on collision markers and multi-dimensional feature data, the robotic arm's working status can be managed in a refined and hierarchical manner. It can issue warnings when the collision risk is low, reduce speed when the risk increases, and actively retreat and stop after a collision is confirmed, thereby maintaining the continuity of operation to the greatest extent while ensuring safety.

[0124] Step 104: When the current working state of the robotic arm is the collision retreat state, a retreat command is generated based on the multi-dimensional feature data to drive the robotic arm to perform a retreat movement.

[0125] In one embodiment, generating a retraction command based on multi-dimensional feature data specifically includes: mapping the calibration joint forces in the multi-dimensional feature data to the Cartesian space of the robotic arm end effector based on the Jacobian matrix to obtain the collision force vector in the Cartesian space, and determining the retraction direction based on the collision force vector; generating a straight retraction trajectory in the Cartesian space based on the retraction direction and a preset retraction distance; determining the retraction speed according to the preset intensity range where the maximum absolute value of the joint force feature in the multi-dimensional feature data is located; calculating the maximum allowable step value of the joint in each control cycle based on the retraction speed and the preset control cycle duration; discretizing the straight retraction trajectory in each control cycle to obtain the desired end effector pose, and solving the desired end effector pose based on inverse kinematics to obtain the desired joint angle; calculating the joint angle increment between the desired joint angle and the joint feedback position in the current control cycle, and limiting the joint angle increment based on the maximum allowable step value to determine the limited joint angle increment; determining the expected joint position value based on the joint angle increment and the joint feedback position in the current control cycle, and outputting the expected joint position value as a retraction command.

[0126] In one embodiment, determining the retraction direction based on the collision force vector specifically includes: calculating the magnitude of the collision force vector; if the magnitude of the collision force vector is not less than a preset effective threshold, then the opposite direction of the collision force vector is determined as the retraction direction; if the magnitude of the collision force vector is less than the effective threshold, then the opposite direction of the tangent of the robot arm's motion trajectory or the opposite direction of the velocity is taken as the retraction direction.

[0127] Specifically, using the current robotic arm's kinematic Jacobian matrix, the calibrated joint force vectors of each joint are converted into six-dimensional end-effector force and torque vectors, denoted as Cartesian collision force vectors. .

[0128] Specifically, the main controller calculates the magnitude of the vector. The module length is then compared with a preset effective threshold. If the module length is greater than or equal to the effective threshold, it indicates that the direction of the collision force is clear and the intensity is sufficient. In this case, the main controller determines the unit vector in the opposite direction of the collision force vector as the retraction direction. This ensures that the robotic arm retracts in a straight line in the opposite direction of the force applied, wherein, This means dividing a vector by its magnitude to obtain a vector with the same direction but a length of 1.

[0129] Preferably, the preset effective threshold is 5 Newtons.

[0130] Specifically, if the magnitude of the collision force vector is less than the effective threshold, such as when only a slight contact occurs at the end effector or the force signal is weak, the main controller degrades to using the kinematic direction as the basis for retraction. The reverse direction of the tangent to the current robotic arm trajectory or the reverse direction of the current velocity is used as the retraction direction. This direction can be obtained by reversing the current desired motion direction from the motion planner. This degradation mechanism ensures that the retraction action can still be executed when the force signal is insufficient to provide a reliable direction, avoiding system lag due to a lack of direction.

[0131] Specifically, after determining the retreat direction, the main controller generates a straight-line retreat trajectory in Cartesian space based on a preset retreat distance. Taking the current end-effector pose as the starting point, the unit vector of the retreat direction is multiplied by the retreat distance to obtain a translation component. This translation component is then superimposed on the current end-effector pose to obtain the target end-effector pose. During this process, the attitude remains unchanged, only the position changes. The straight-line retreat trajectory is a straight line in Cartesian space from the current end-effector pose to the target end-effector pose. This trajectory will be discretized and interpolated point by point within the subsequent 500Hz control cycle.

[0132] Preferably, the preset retraction distance is 0.05 meters.

[0133] Specifically, the retreat speed is selected in stages based on the current collision intensity, i.e., the maximum absolute value of the joint force. The main controller compares the value of the maximum absolute value of the joint force with a preset intensity range: if the value is in the mild collision intensity range, a fast retreat speed is selected to quickly escape the obstacle; if it is in the moderate collision intensity range, a medium retreat speed is selected to balance efficiency and safety; if it is greater than or equal to the severe collision intensity range, a low retreat speed is selected to avoid joint impact. Through this graded speed adjustment, the system can adaptively adjust the retreat speed according to the severity of the collision.

[0134] Preferably, the light collision intensity range is less than 20 units; the moderate collision intensity range is between 20 and 50 units; the heavy collision intensity range is greater than 50 units; the rapid retreat speed is 50 degrees / second, the medium retreat speed is 30 degrees / second, and the low retreat speed is 10 degrees / second.

[0135] Specifically, in order to ensure that only a small step is moved in each control cycle and to ensure smooth movement, the main controller first converts the retraction speed from degrees / second to radians / second and then divides it by the control frequency of 500Hz to obtain the maximum allowable step value of the joint in each control cycle.

[0136] Specifically, in each control cycle, the main controller discretizes the linear retreat trajectory, calculates the desired end-effector pose based on the current normalized parameters, and then calls the inverse kinematics solver to convert the desired pose into desired joint angles. After obtaining the desired joint angles, the current joint feedback position is subtracted from the desired joint angles to obtain the joint angle increment between the desired joint angle and the current joint feedback position. Subsequently, the joint angle increment for each joint is limited: if the absolute value of the joint angle increment exceeds the maximum allowable step value, it is clamped to the maximum value; if the absolute value of the joint angle increment does not exceed the maximum allowable step value, it remains unchanged. Finally, the limited joint angle increment is added to the current joint feedback position to obtain the desired joint position value for the current cycle, which is output as a retreat command to the joint servo driver.

[0137] Specifically, by taking one step per cycle and solving while limiting the amplitude as described above, the main controller ensures that the robotic arm retracts smoothly along the predetermined straight trajectory, the end-effector posture remains unchanged, and there are no sudden joint changes or secondary collisions.

[0138] Specifically, if the inverse kinematics solution fails, for example, due to exceeding the workspace or a singular configuration, the rollback instruction retains the previous cycle value and generates a warning; the rollback process continues until the trajectory parameters reach the endpoint or meet the positioning conditions.

[0139] In one embodiment, after generating the retraction command, the method further includes: acquiring the retraction operation duration in real time and comparing it with a preset timeout duration; if the retraction operation duration is greater than the preset timeout duration, generating a retraction timeout flag; wherein the retraction operation duration is obtained by timing when the robotic arm enters the collision retraction state; in each control cycle, determining whether the maximum deviation between the expected joint position value in the retraction command and the joint feedback position in the current control cycle is less than a preset position tolerance value; if the number of consecutive control cycles in which the maximum deviation between the retraction command and the joint feedback position in the current control cycle is not greater than the position tolerance value reaches a preset number of stable cycles, and the retraction operation duration is not greater than the preset timeout duration, generating a retraction completion flag.

[0140] Specifically, during the process of generating rollback instructions and issuing them cycle by cycle, the main controller simultaneously executes rollback convergence determination and timeout protection logic to determine whether the rollback has been successfully completed or needs to be forcibly terminated.

[0141] Specifically, when the robotic arm enters the collision retreat state, the main controller starts an internal timer to record the duration of the retreat operation in real time, which is recorded as the retreat operation duration. In each control cycle, the main controller compares this retreat operation duration with a preset timeout duration. If the retreat operation duration exceeds the preset timeout duration, it indicates that the robotic arm failed to complete the retreat movement within the specified time, for example, due to obstacle obstruction or abnormal trajectory planning. In this case, the main controller generates a retreat timeout flag. Once this retreat timeout flag is generated, it triggers the state machine to switch from the collision retreat state to the collision stop state to avoid unknown risks caused by unlimited retreat.

[0142] Specifically, in each control cycle, the main controller also calculates the deviation between the expected joint position in the backoff command and the joint feedback position in the current control cycle. Specifically, it calculates the absolute value of the difference between the expected position and the feedback position for each joint, and then takes the maximum absolute value among all joints as the maximum deviation value. The maximum deviation value is compared with the preset position tolerance value. If the maximum deviation value is less than or equal to the position tolerance value, it is determined that the position condition is met in the current cycle; otherwise, it is determined that it is not met.

[0143] Specifically, to ensure the robotic arm truly stabilizes at the target position rather than merely passing by, the main controller maintains a continuous and stable counter. When the number of control cycles that continuously meet the positioning condition reaches a preset number of stable cycles, and the retraction operation duration does not exceed a preset timeout duration (i.e., no timeout has been triggered), the main controller generates a retraction completion flag. Conversely, if the positioning condition is not met in any cycle during continuous counting, the continuous and stable counter is immediately reset to zero and counting restarts.

[0144] Specifically, after generating the retraction completion flag or the retraction timeout flag, the main controller transmits these two flags to the state management module. Based on this, the state management module switches the current working state of the robotic arm from the collision retraction state to the collision stop state, stops issuing active motion commands, and waits for subsequent external recovery commands.

[0145] Through the aforementioned convergence determination and timeout protection mechanisms, reliable confirmation of completion is ensured in normal rollback scenarios, while also providing a safety net for abnormal situations, further enhancing the robustness and security of the system.

[0146] Example 2, see Figure 2 , Figure 2 This is a schematic diagram of one embodiment of a collision handling device for a robotic arm provided in this application. Corresponding to the above-described collision handling method for a robotic arm, this application also provides a collision handling device for a robotic arm. This collision handling device includes modules for executing the above-described collision handling method for a robotic arm, and can be configured in a desktop computer, tablet computer, laptop computer, or other terminal. Specifically, the collision handling device includes a data processing module 201, a collision detection module 202, a state management module 203, and a retreat control module 204.

[0147] The data processing module 201 is used to acquire multi-dimensional sensing data of the robotic arm and perform fusion filtering processing on the multi-dimensional sensing data to obtain multi-dimensional feature data.

[0148] The collision detection module 202 is used to periodically update the dynamic activity thresholds corresponding to each dimension in the multi-dimensional feature data, and to perform multi-dimensional joint comparison between the multi-dimensional feature data and the dynamic activity thresholds respectively, and output a collision identifier.

[0149] The state management module 203 is used to determine the current working state of the robotic arm based on the collision identifier and the multi-dimensional feature data.

[0150] The retraction control module 204 is used to generate a retraction command based on the multi-dimensional feature data when the current working state of the robotic arm is a collision retraction state, so as to drive the robotic arm to perform a retraction movement.

[0151] In one embodiment, the data processing module 201 is used to acquire multi-dimensional sensing data of the robotic arm and perform fusion filtering on the multi-dimensional sensing data to obtain multi-dimensional feature data. Specifically, this includes: synchronously acquiring multi-dimensional sensing data of the robotic arm at a preset high-frequency sampling period, wherein the multi-dimensional sensing data includes at least the joint force, joint feedback position, joint feedback speed, joint current, joint command position, and end-effector six-dimensional perturbation estimate of each joint in the robotic arm; adjusting the joint force based on a pre-stored static bias of the joint force, and adjusting the joint current based on a pre-stored static bias of the joint current to obtain adjusted joint force data. The process involves adjusting the joint force and adjusting the joint current; calculating the position deviation between the joint command position and the joint feedback position; weighting and fusing the adjusted joint force, the joint feedback velocity, and the position deviation to construct a fused observation value of the joint force; performing Kalman filtering on the fused observation value of the joint force to output the calibrated joint force; performing Kalman filtering on the adjusted joint current and the estimated end-effector six-dimensional perturbation value to output the calibrated joint current and the estimated end-effector six-dimensional perturbation value; and using the calibrated joint force, the calibrated joint current, the estimated end-effector six-dimensional perturbation value, and the position deviation as the multi-dimensional feature data.

[0152] In one embodiment, the collision detection module 202 is used to periodically update the dynamic activity thresholds corresponding to each dimension in the multi-dimensional feature data. Specifically, it includes: maintaining a sliding window for each feature quantity corresponding to each dimension in the multi-dimensional feature data, wherein the sliding window is used to store historical feature quantities corresponding to a preset number of historical control cycles closest to the current control cycle, and storing the current feature quantity calculated in the current control cycle into the corresponding sliding window in each control cycle to update the historical feature quantity in the sliding window; calculating the statistical feature value corresponding to the historical feature quantity in the sliding window corresponding to each feature quantity in each control cycle, and calculating the candidate threshold corresponding to each feature quantity based on the statistical feature value; comparing the candidate threshold corresponding to each feature quantity with the preset lower limit threshold corresponding to the feature quantity, and determining the dynamic activity threshold of each feature quantity in the current cycle based on the comparison result.

[0153] In one embodiment, the collision detection module 202 is used to perform multi-dimensional joint comparison of the multi-dimensional feature data with the dynamic activity threshold and output a collision identifier. Specifically, it includes: comparing the value of the feature quantity of each dimension in the multi-dimensional feature data of the current control cycle with the dynamic activity threshold corresponding to the feature quantity; determining whether each dimension in the multi-dimensional feature data meets the preset candidate conditions based on the comparison result; counting the number of dimensions that meet the candidate conditions in the current control cycle to obtain the total number of candidate dimensions; if the total number of candidate dimensions is not less than the preset minimum number of joint triggering dimensions, generating a first collision candidate identifier; otherwise, generating a second collision candidate identifier; determining whether the current control cycle is the cooling cycle of the robotic arm; if so, outputting a second collision confirmation identifier; otherwise, determining whether the number of control cycles that continuously generate the first collision candidate identifier has reached the preset number of de-jittering cycles; if so, outputting a first collision confirmation identifier; otherwise, outputting a second collision confirmation identifier; and using the first collision candidate identifier or the second collision candidate identifier, as well as the first collision confirmation identifier or the second collision confirmation identifier, as collision identifiers and outputting the collision identifiers.

[0154] In one embodiment, the multi-dimensional feature data includes at least the joint force dimension, the terminal six-dimensional perturbation estimation value dimension, the joint current dimension, and the position deviation dimension. The feature quantities of the joint force dimension include at least the maximum absolute value feature quantity of the joint force and the L2 norm feature quantity of the joint force. The feature quantities of the terminal six-dimensional perturbation estimation value dimension include at least the maximum absolute value feature quantity of the terminal six-dimensional perturbation estimation value and the L2 norm feature quantity of the terminal six-dimensional perturbation estimation value. The feature quantities of the joint current dimension include at least the maximum absolute value feature quantity of the joint current and the L2 norm feature quantity of the joint current. The feature quantities of the position deviation dimension include at least the maximum absolute value feature quantity of the position deviation.

[0155] In one embodiment, the collision detection module 202 is used to generate a first collision candidate identifier if the total number of candidate dimensions is not less than a preset minimum number of joint triggering dimensions; otherwise, after generating a second collision candidate identifier, it further includes: determining whether the value of the maximum absolute value feature of the joint force in the joint force dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and a first preset strong triggering multiple; and determining whether the value of the maximum absolute value feature of the end-six-dimensional disturbance estimation value in the end-six-dimensional disturbance estimation value dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the end-six-dimensional disturbance estimation value dimension and a second preset strong triggering multiple; if the value of the maximum absolute value feature of the joint force exceeds the dynamic activity threshold corresponding to the joint force dimension... If the value of the maximum absolute value feature of the six-dimensional disturbance estimate exceeds the product of the dynamic activity threshold corresponding to the six-dimensional disturbance estimate dimension and the second preset strong triggering multiple, then it is determined whether there is a second collision candidate identifier in the current collision identifier. If so, the second collision candidate identifier is modified to the first collision candidate identifier; otherwise, the current collision identifier is not modified. If the value of the maximum absolute value feature of the joint force does not exceed the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple, and the value of the maximum absolute value feature of the six-dimensional disturbance estimate does not exceed the product of the dynamic activity threshold corresponding to the six-dimensional disturbance estimate dimension and the second preset strong triggering multiple, then the current collision identifier is not modified.

[0156] In one embodiment, the working states of the robotic arm include normal operation state, early warning state, deceleration operation state, collision retreat state, and collision stop state.

[0157] In one embodiment, the state management module 203 is used to determine the current working state of the robotic arm based on the collision identifier and the multi-dimensional feature data, specifically including: when the collision identifier includes a first collision confirmation identifier, determining the current working state of the robotic arm as the collision retreat state; when the collision identifier includes a second collision confirmation identifier and a first collision candidate identifier, determining the current working state of the robotic arm as the deceleration running state; when the collision identifier includes a second collision confirmation identifier and a second collision candidate identifier, and the value of the maximum absolute value feature of the joint force in the multi-dimensional feature data is greater than the product of a preset first threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, determining the current working state of the robotic arm as the warning state; when the current working state of the robotic arm is the warning state, and the maximum absolute value feature of the joint force is greater than the product of the ... preset first threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, determining the current working state of the robotic arm as the warning state; when the current working state of the robotic arm is the warning state, and the maximum absolute value feature of the joint force is greater than the preset first threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, determining the current working state of the robotic arm as the warning state. When the absolute value of the feature quantity is less than the product of a preset second threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, the current working state of the robotic arm is switched back to the normal operating state, wherein the second threshold ratio is less than the first threshold ratio; when the current working state of the robotic arm is the deceleration operating state, and the number of control cycles for continuously generating the second collision candidate identifier reaches a preset number of continuous cycles, the current working state of the robotic arm is switched back to the normal operating state; when the current working state of the robotic arm is the collision retreat state, and a retreat completion flag or a retreat timeout flag is received, the current working state of the robotic arm is switched to the collision stop state; when the current working state of the robotic arm is the collision stop state, and an external recovery command is received, the current working state of the robotic arm is switched back to the normal operating state.

[0158] In one embodiment, the retraction control module 204 is used to generate a retraction command based on the multi-dimensional feature data, specifically including: mapping the calibration joint force in the multi-dimensional feature data to the Cartesian space of the robotic arm end effector based on the Jacobian matrix to obtain the collision force vector in the Cartesian space, and determining the retraction direction based on the collision force vector; generating a straight retraction trajectory in the Cartesian space based on the retraction direction and a preset retraction distance; determining the retraction speed according to the preset intensity range where the maximum absolute value feature of the joint force in the multi-dimensional feature data is located; and calculating the retraction speed and the preset control cycle duration. Calculate the maximum permissible step value of the joint within each control cycle; in each control cycle, discretize the linear retreat trajectory to obtain the desired end pose, and solve the desired end pose based on inverse kinematics to obtain the desired joint angle; calculate the joint angle increment between the desired joint angle and the joint feedback position of the current control cycle, and limit the joint angle increment based on the maximum permissible step value to determine the limited joint angle increment; determine the expected joint position value based on the joint angle increment and the joint feedback position of the current control cycle, and output the expected joint position value as a retreat command.

[0159] The collision handling device for the robotic arm described above can implement the collision handling method for the robotic arm in the above method embodiments. The options in the above method embodiments are also applicable to this embodiment, and will not be detailed here.

[0160] like Figure 3 As shown, Figure 3 This is a schematic diagram of the structure of a computer device provided in this application; it includes a processor 111, a communication interface 112, a memory 113 and a communication bus 114, wherein the processor 111, the communication interface 112 and the memory 113 communicate with each other through the communication bus 114, and the memory 113 is used to store computer programs.

[0161] In one embodiment of this application, the processor 111, when executing the program stored in the memory 113, implements the collision handling method of the robotic arm provided in any of the foregoing method embodiments.

[0162] It will be understood by those skilled in the art that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program may be stored in a storage medium, which is a computer-readable storage medium. The computer program is executed by at least one processor in the computer system to implement the process steps of the embodiments of the above methods.

[0163] Therefore, embodiments of this application also provide a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the collision handling method for a robotic arm as provided in any of the foregoing method embodiments.

[0164] The storage medium is a physical, non-transient storage medium, such as a USB flash drive, external hard drive, read-only memory (ROM), magnetic disk, or optical disk, or any other physical storage medium capable of storing program code. The computer-readable storage medium can be non-volatile or volatile.

[0165] 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, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. 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 implementations should not be considered beyond the scope of this application.

[0166] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative. For example, the division of each unit is merely a logical functional division, and there may be other division methods in actual implementation. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed.

[0167] The steps in the methods of this application embodiment can be adjusted, merged, or deleted according to actual needs. The units in the apparatus of this application embodiment can be merged, divided, or deleted according to actual needs. Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0168] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, a terminal, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application.

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

[0170] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Since these modifications and variations fall within the scope of the claims and their equivalents, this application also intends to include these modifications and variations.

[0171] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in this application, and these modifications or substitutions should all be covered 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 handling method for a robotic arm, characterized in that, include: The multi-dimensional sensing data of the robotic arm is acquired, and the multi-dimensional sensing data is fused and filtered to obtain multi-dimensional feature data. The dynamic activity thresholds corresponding to each dimension in the multi-dimensional feature data are periodically updated, and the multi-dimensional feature data is compared with the dynamic activity thresholds in a multi-dimensional joint comparison to output a collision identifier. The current working state of the robotic arm is determined based on the collision marker and the multi-dimensional feature data. When the current working state of the robotic arm is a collision retreat state, a retreat command is generated based on the multi-dimensional feature data to drive the robotic arm to perform a retreat movement.

2. The method according to claim 1, characterized in that, The process of acquiring multi-dimensional sensing data from the robotic arm and performing fusion filtering on the multi-dimensional sensing data to obtain multi-dimensional feature data specifically includes: Multi-dimensional sensing data of the robotic arm are synchronously collected at a preset high-frequency sampling period. The multi-dimensional sensing data includes at least the joint force, joint feedback position, joint feedback speed, joint current, joint command position and end-effector six-dimensional disturbance estimate of each joint in the robotic arm. The joint force is adjusted based on a pre-stored static bias of the joint force, and the joint current is adjusted based on a pre-stored static bias of the joint current, so as to obtain the adjusted joint force and the adjusted joint current. Calculate the positional deviation between the joint command position and the joint feedback position, perform weighted fusion of the adjusted joint force, the joint feedback velocity and the positional deviation to construct a fused observation value of the joint force, and perform Kalman filtering on the fused observation value of the joint force to output the calibrated joint force; Kalman filtering is applied to the adjusted joint current and the estimated six-dimensional end disturbance, respectively, to output the calibrated joint current and the estimated six-dimensional end disturbance. The calibration joint force, the calibration joint current, the estimated six-dimensional perturbation value at the calibration end, and the position deviation are used as the multi-dimensional feature data.

3. The method according to claim 1, characterized in that, The periodic updating of the dynamic activity thresholds corresponding to each dimension in the multi-dimensional feature data specifically includes: A sliding window is maintained for each feature quantity corresponding to each dimension in the multi-dimensional feature data. The sliding window is used to store the historical feature quantities corresponding to the preset number of historical control cycles closest to the current control cycle. In each control cycle, the current feature quantity calculated in the current control cycle is stored into the corresponding sliding window to update the historical feature quantity in the sliding window. In each control cycle, the statistical feature value corresponding to the historical feature value within the sliding window corresponding to each feature value is calculated, and the candidate threshold corresponding to each feature value is calculated based on the statistical feature value. The candidate threshold corresponding to each feature is compared with the preset lower limit threshold corresponding to that feature. Based on the comparison results, the dynamic activity threshold of each feature in the current period is determined.

4. The method according to claim 1, characterized in that, The step of performing a multi-dimensional joint comparison between the multi-dimensional feature data and the dynamic activity threshold, and outputting a collision identifier, specifically includes: The values ​​of the feature quantities of each dimension in the multi-dimensional feature data of the current control cycle are compared with the dynamic activity threshold corresponding to the feature quantity. Based on the comparison results, it is determined whether each dimension in the multi-dimensional feature data meets the preset candidate conditions. The total number of candidate dimensions is obtained by counting the number of dimensions that meet the candidate conditions in the current control cycle. If the total number of candidate dimensions is not less than the preset minimum number of joint triggering dimensions, then a first collision candidate identifier is generated; otherwise, a second collision candidate identifier is generated. Determine whether the current control cycle is the cooling cycle of the robotic arm. If so, output the second collision confirmation flag. Otherwise, determine whether the number of control cycles that continuously generate the first collision candidate flag has reached the preset number of de-jittering cycles. If so, output the first collision confirmation flag. Otherwise, output the second collision confirmation flag. The first collision candidate identifier or the second collision candidate identifier, and the first collision confirmation identifier or the second collision determination identifier are used as collision identifiers, and the collision identifiers are output.

5. The method according to claim 4, characterized in that, The multi-dimensional feature data includes at least the joint force dimension, the terminal six-dimensional perturbation estimation dimension, the joint current dimension, and the position deviation dimension. Specifically, the feature quantities of the joint force dimension include at least the maximum absolute value feature quantity of the joint force and the L2 norm feature quantity of the joint force; the feature quantities of the terminal six-dimensional perturbation estimation dimension include at least the maximum absolute value feature quantity of the terminal six-dimensional perturbation estimation value and the L2 norm feature quantity of the terminal six-dimensional perturbation estimation value; the feature quantities of the joint current dimension include at least the maximum absolute value feature quantity of the joint current and the L2 norm feature quantity of the joint current; and the feature quantities of the position deviation dimension include at least the maximum absolute value feature quantity of the position deviation. If the total number of candidate dimensions is not less than the preset minimum number of joint triggering dimensions, then a first collision candidate identifier is generated; otherwise, after generating a second collision candidate identifier, the process further includes: Determine whether the value of the maximum absolute value feature of the joint force in the joint force dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple; and determine whether the value of the maximum absolute value feature of the end six-dimensional disturbance estimation value in the end six-dimensional disturbance estimation value dimension in the current control cycle exceeds the product of the dynamic activity threshold corresponding to the end six-dimensional disturbance estimation value dimension and the second preset strong triggering multiple. If the value of the maximum absolute value feature of the joint force exceeds the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple, or if the value of the maximum absolute value feature of the end six-dimensional perturbation estimate exceeds the product of the dynamic activity threshold corresponding to the end six-dimensional perturbation estimate dimension and the second preset strong triggering multiple, then it is determined whether there is a second collision candidate identifier in the current collision identifier. If so, the second collision candidate identifier is modified to the first collision candidate identifier; otherwise, the current collision identifier is not modified. If the value of the maximum absolute value feature of the joint force does not exceed the product of the dynamic activity threshold corresponding to the joint force dimension and the first preset strong triggering multiple, and the value of the maximum absolute value feature of the end six-dimensional disturbance estimate does not exceed the product of the dynamic activity threshold corresponding to the end six-dimensional disturbance estimate dimension and the second preset strong triggering multiple, then the current collision identifier will not be modified.

6. The method according to claim 5, characterized in that, The working states of the robotic arm include normal operation, early warning, deceleration, collision retreat, and collision stop. Determining the current working state of the robotic arm based on the collision marker and the multi-dimensional feature data specifically includes: When the collision identifier includes a first collision confirmation identifier, the current working state of the robotic arm is determined to be the collision retreat state; When the collision identifier includes a second collision confirmation identifier and a first collision candidate identifier, the current working state of the robotic arm is determined to be the deceleration operation state. When the collision identifier includes a second collision confirmation identifier and a second collision candidate identifier, and the value of the maximum absolute value feature of the joint force in the multidimensional feature data is greater than the product of a preset first threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, the current working state of the robotic arm is determined to be the warning state. When the current working state of the robotic arm is the warning state, and the value of the maximum absolute value of the joint force is less than the product of the preset second threshold ratio and the dynamic activity threshold corresponding to the joint force dimension, the current working state of the robotic arm is switched back to the normal working state, wherein the second threshold ratio is less than the first threshold ratio. When the current working state of the robotic arm is the decelerated operation state, and the number of control cycles for continuously generating the second collision candidate identifier reaches the preset number of continuous cycles, the current working state of the robotic arm is switched back to the normal operation state. When the current working state of the robotic arm is the collision retreat state, and a retreat completion flag or a retreat timeout flag is received, the current working state of the robotic arm is switched to the collision stop state. When the current working state of the robotic arm is the collision-stopped state and an external recovery command is received, the current working state of the robotic arm is switched back to the normal operating state.

7. The method according to claim 5, characterized in that, The process of generating a rollback instruction based on the multi-dimensional feature data specifically includes: Based on the Jacobian matrix, the calibration joint forces in the multi-dimensional feature data are mapped to the Cartesian space at the end of the robotic arm to obtain the collision force vector in the Cartesian space, and the retraction direction is determined based on the collision force vector. A straight-line retreat trajectory in Cartesian space is generated based on the retreat direction and the preset retreat distance; The retraction speed is determined based on the preset intensity range in which the maximum absolute value of the joint force in the multi-dimensional feature data is located; Based on the retraction speed and the preset control cycle duration, the maximum allowable step value of the joint within each control cycle is calculated; In each control cycle, the straight retreat trajectory is discretized to obtain the desired end pose, and the desired joint angle is obtained by solving the desired end pose based on inverse kinematics. Calculate the joint angle increment between the desired joint angle and the joint feedback position in the current control cycle, and limit the joint angle increment based on the maximum allowable step value to determine the limit joint angle increment; Based on the joint angle increment and the joint feedback position of the current control cycle, the expected value of the joint position is determined, and the expected value of the joint position is output as a backoff command.

8. A collision handling device for a robotic arm, characterized in that, include: Data processing module, collision detection module, status management module, and backoff control module; The data processing module is used to acquire multi-dimensional sensing data of the robotic arm and perform fusion filtering on the multi-dimensional sensing data to obtain multi-dimensional feature data. The collision detection module is used to periodically update the dynamic activity thresholds corresponding to each dimension in the multi-dimensional feature data, and to perform multi-dimensional joint comparison between the multi-dimensional feature data and the dynamic activity thresholds respectively, and output a collision identifier. The state management module is used to determine the current working state of the robotic arm based on the collision identifier and the multi-dimensional feature data. The retraction control module is used to generate a retraction command based on the multi-dimensional feature data when the current working state of the robotic arm is a collision retraction state, so as to drive the robotic arm to perform a retraction movement.

9. A computer device, characterized in that, The computer device includes a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the method as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The storage medium stores a computer program that, when executed by a processor, can implement the method as described in any one of claims 1-7.

Images