Mechanical arm collision early warning method, control device and mechanical arm
Through the dual-mode trigger mechanism of joint motor current change rate and end effector position deviation, combined with sliding window algorithm and adaptive threshold adjustment, the accuracy and reliability of robotic arm collision warning are solved, and efficient and safe collision warning is achieved.
Patent Information
- Application Number
- CN202510193746.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-02-21
- Publication Date
- 2025-07-08
AI Technical Summary
The existing robotic arm collision warning technology lacks accuracy and reliability, making it difficult to accurately identify minor collisions or low-rigid contacts in complex environments, affecting the operating efficiency and safety of the robotic arm.
A dual-mode trigger mechanism based on the current change rate of the motor of the robot joint and the position deviation of the end effector is adopted, and a sliding window algorithm, low-pass filtering, adaptive threshold adjustment and reverse kinematic interpolation method is combined to achieve high-precision collision warning.
Improve the accuracy and reliability of robotic arm collision warning, reduce hardware costs, and ensure operational safety and efficiency.
Smart Images
Figure CN120269544A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of collision warning, and in particular to a method for warning of robotic arm collisions, a control device, and a robotic arm. Background Art
[0002] In the fields of modern industrial production and automation, the wide application of robotic arms has greatly improved production efficiency and quality. However, the resulting collision risks may not only cause damage to the robotic arms themselves and surrounding equipment, increasing maintenance costs and downtime, but also pose a serious threat to the personal safety of operators. Therefore, robotic arm collision warning is of great significance.
[0003] Currently, the existing technologies for robotic arm collision warning include force sensor detection, vision system recognition, encoder feedback, etc. Force sensor detection judges collisions based on changes in force and torque, but its accuracy and reliability are affected by the environment, and it has poor detection effects for minor collisions or low-stiffness contacts. The vision system can identify the environment and the positions of objects, but it is sensitive to light, has a large amount of algorithm calculation and poor real-time performance, and is difficult to accurately identify in complex environments. Encoder feedback can only reflect the joint positions, is ineffective for warning of non-joint movement collisions such as external impacts, and cannot sense object contacts. These deficiencies limit the accuracy and reliability of warning, and a better solution is urgently needed.
[0004] Therefore, providing a method for warning of robotic arm collisions has important practical significance for improving the operation efficiency of robotic arms and reducing the maintenance costs of robotic arms. Summary of the Invention
[0005] This application provides a method for warning of robotic arm collisions, a control device, and a robotic arm. This method is based on the sensors of the robotic arm body and adopts a dual-mode triggering mechanism, which improves the warning accuracy rate and reduces the warning cost without the need for additional hardware sensors.
[0006] In a first aspect, a method for warning of robotic arm collisions is provided, and the method includes:
[0007] S1: Real-time collect the current data of each joint motor of the robotic arm, and calculate the current change rate Δi of the current data through a sliding window algorithm;
[0008] S2: Based on the preset motion trajectory of the robotic arm, real-time collect the actual position information of the end effector of the robotic arm, and calculate the deviation amount α between the actual position information and the theoretical position information of the end effector;
[0009] S3: Determine whether the robotic arm needs to issue a collision warning based on the current change rate Δi and the deviation α: If the current change rate Δi of at least one joint motor exceeds the first threshold and the deviation α is less than the second threshold, it is determined that the load is abnormal and a speed reduction warning is triggered; If the current change rate Δi of at least one joint motor exceeds the first threshold and the deviation α exceeds the second threshold, it is determined that a collision has occurred, an emergency stop is triggered, and the obstacle area is marked.
[0010] It should be understood that the robotic arm includes at least one joint motor. When the robotic arm is subjected to additional resistance or load changes, the current of the joint motor will change accordingly. Therefore, by calculating the current change rate Δi, the change in the load of the robotic arm can be reflected.
[0011] It should be understood that the end effector is the end tool of the robotic arm, which directly interacts with the environment or workpiece to complete the specified task. The end effector is the "hand" of the robotic arm to perform functions and is also the part with the highest collision risk. Therefore, in this application, the deviation α between the actual position and the theoretical position of the end effector is selected as one of the criteria for judging whether a collision occurs on the robotic arm.
[0012] It should also be understood that when the current change rate Δi of at least one joint motor exceeds the first threshold and the deviation α is less than the second threshold, this means that the robotic arm may encounter an additional load, but it has not yet caused a large deviation in the motion trajectory. At this time, it is determined that the load is abnormal, a speed reduction warning is triggered, the operator is reminded to pay attention and take corresponding measures, and at the same time, the operating speed of the robotic arm is reduced to avoid potential dangers; If the current change rate Δi of at least one joint motor exceeds the first threshold and the deviation α also exceeds the second threshold, in this case, not only is the load of the robotic arm abnormal, but also the motion trajectory significantly deviates from the expectation, then it is determined that a collision has occurred. At this time, an emergency stop is triggered to immediately stop the movement of the robotic arm to prevent further damage and danger, and the obstacle area is marked to provide a reference for subsequent processing and analysis.
[0013] Combined with the first aspect, in some implementation manners of the first aspect, the sliding window algorithm uses a dynamic window width, and the window width is inversely proportional to the motion speed of the robotic arm.
[0014] It should be understood that the sliding window algorithm is an algorithm idea for operating on sequence data such as arrays and strings. In this application, a continuous segment of data is selected from the collected current data sequence of the robotic arm joint motor as a window. As new data is continuously collected, the window will slide on the data sequence at a certain step size. Within this window, the current change rate Δi can be calculated and used as one of the bases for judging the operating state of the robotic arm.
[0015] It should also be understood that the selected dynamic window width in this application refers to the size of the sliding window, which is not fixed but will be dynamically adjusted according to certain conditions. In the robotic arm collision warning method, the window width is inversely proportional to the movement speed of the robotic arm: when the robotic arm moves fast, the window width becomes smaller, so that the rapidly changing current data can be captured more precisely to promptly detect current anomalies caused by collisions or sudden load changes; when the robotic arm moves slowly, the window width becomes larger, enabling more sufficient processing of relatively stable current data, filtering out some noise interference, and making the calculation results more accurately reflect the actual load conditions.
[0016] Combined with the first aspect, in some implementation manners of the first aspect, the deviation α is obtained by real-time calculation based on the encoder feedback data of the robotic arm and the forward kinematic model.
[0017] It should be understood that the encoder, as a key sensor for measuring mechanical motion parameters, can accurately measure and feedback information such as the angular displacement and angular velocity of the robotic arm joints. The forward kinematic model is a mathematical model that describes the mapping relationship of the robotic arm from the joint space (constituted by the angles or displacements of each joint) to the Cartesian space (i.e., the position and posture of the end effector of the robotic arm in three-dimensional space). Its core function is to calculate the position and posture of the end effector in the Cartesian space based on the known angles or displacements of each joint of the robotic arm. This application calculates the offset α between the actual position and the theoretical position of the end effector of the robotic arm by using the existing encoder data, without the need to install external sensors, simplifying the operation process and reducing the hardware cost.
[0018] Combined with the first aspect, in some implementation manners of the first aspect, a low-pass filtering operation is added when collecting current data to filter the noise of the joint motors.
[0019] Combined with the first aspect, in some implementation manners of the first aspect, the setting of the first threshold and the second threshold is based on historical collision data and the real-time load weight, and is dynamically adjusted through an adaptive algorithm.
[0020] It should be understood that historical collision data can reflect the parameter range during past collisions, providing a reference basis for threshold setting; the real-time load weight will change the working state of the motor and the movement deviation of the robotic arm. Adjusting the threshold based on it can fit the actual working conditions. This application uses an adaptive algorithm to automatically adjust the threshold according to this real-time information, enabling the collision warning system to be more intelligent and accurate, adapting to different loads and working environments, and avoiding the problem of insufficient adaptability of fixed thresholds in light-load or heavy-load scenarios.
[0021] Combined with the first aspect, in some implementation manners of the first aspect, the method further includes autonomous obstacle avoidance path planning: after the robotic arm stops urgently, the coordinates of the obstacle are reversely calculated based on the collision position, and a local correction trajectory that bypasses the obstacle coordinates is generated to automatically resume the operation.
[0022] In combination with the first aspect, in some implementations of the first aspect, inverse kinematics interpolation is used to generate a local correction trajectory for autonomous obstacle avoidance path planning.
[0023] It should be understood that for the inverse kinematics interpolation method, a series of intermediate desired positions of the end effector are first determined based on the obstacle coordinates and the target position to be reached by the robotic arm, and then the joint angles corresponding to each intermediate position are calculated using inverse kinematics. Interpolation calculations are performed between these joint angles to smoothly transition and generate a continuous sequence of joint angle changes, thereby obtaining a local correction trajectory that enables the robotic arm to avoid obstacles and safely resume operation.
[0024] In combination with the first aspect, in some implementations of the first aspect, during the generation of the local correction trajectory, the gradient descent method is used for trajectory optimization.
[0025] It should be understood that the gradient descent method is an iterative optimization algorithm whose goal is to find the minimum value of a function. In this application, when the inverse kinematics interpolation method is used to generate the local correction trajectory, the gradient descent method is used for trajectory optimization. Through multiple iterations, the trajectory is continuously adjusted to gradually make the trajectory tend to have the minimum cost, and finally a better local correction trajectory is obtained, improving the efficiency and safety of the robotic arm's obstacle avoidance movement.
[0026] In a second aspect, a control device is provided. The device includes a processor and a memory. The processor is coupled to the memory. The memory is used to store computer programs or instructions, and the processor is used to execute the computer programs or instructions in the memory, so that the method described in any implementation of the first aspect is executed.
[0027] In a third aspect, a robotic arm is provided. The robotic arm includes the control device described in the second aspect. Description of the Drawings
[0028] Figure 1 It is a flowchart of a method for robotic arm collision warning provided by an embodiment of this application.
[0029] Figure 2 It is a flowchart of a method for dynamically adjusting the threshold by an adaptive algorithm provided by an embodiment of this application. Detailed Embodiments
[0030] The terms used in the following embodiments are only for the purpose of describing specific embodiments and are not intended to limit the present application. As used in the specification and appended claims of the present application, the singular forms "a", "an", "", "the above", "the", and "this" are also intended to include expressions such as "one or more", unless the context clearly indicates otherwise. It should also be understood that in the following embodiments of the present application, "at least one" and "one or more" mean one, two, or more than two. The term "and / or" is used to describe the association relationship of associated objects and indicates that three relationships may exist; for example, A and / or B may mean: A exists alone, A and B exist simultaneously, and B exists alone, where A and B may be singular or plural. The character " / " generally indicates that the associated objects before and after are in an "or" relationship.
[0031] Reference to "one embodiment" or "some embodiments" etc. described in this specification means that a specific feature, structure, or characteristic described in connection with that embodiment is included in one or more embodiments of the present application. Thus, statements such as "in one embodiment", "in some embodiments", "in other some embodiments", "in still other embodiments" etc. that appear in different places in this specification do not necessarily all refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in other ways. The terms "comprising", "including", "having" and their variants all mean "including but not limited to", unless otherwise specifically emphasized in other ways.
[0032] In the field of modern industrial production and automation, the widespread application of robotic arms has greatly improved production efficiency and quality. However, the resulting collision risk may not only cause damage to the robotic arm itself and surrounding equipment, increasing maintenance costs and downtime, but also pose a serious threat to the personal safety of operators. Therefore, robotic arm collision warning is of great significance.
[0033] The present application provides a robotic arm collision warning method, a control device, and a robotic arm. The method performs collision warning based on the current change of the robotic arm joint motors and the position deviation of the end effector, and can achieve high-precision collision warning on the premise of not adding additional hardware.
[0034] Figure 1 This is a flowchart for implementing a robotic arm collision warning method provided by an embodiment of the present application. In some examples, the method includes:
[0035] S1: Real-time collect the current data of each joint motor of the robotic arm, and calculate the current change rate Δi of the current data through a sliding window algorithm;
[0036] S2: Based on the preset motion trajectory of the robotic arm, the actual position information of the end effector of the robotic arm is collected in real time, and the deviation α between the actual position information and the theoretical position information of the end effector is calculated.
[0037] S3: Based on the current change rate Δi and the deviation α, it is judged whether the robotic arm needs to give a collision warning.
[0038] In a possible implementation, the judgment rule for the robotic arm collision warning is as follows: If the current change rate Δi of at least one joint motor exceeds the first threshold and the deviation α is less than the second threshold, it is determined that the load is abnormal and a speed reduction warning is triggered; If the current change rate Δi of at least one joint motor exceeds the first threshold and the deviation α exceeds the second threshold, it is determined that a collision has occurred, an emergency stop is triggered, and the obstacle area is marked.
[0039] In some examples, the sliding window algorithm uses a dynamic window width, and the window width is inversely proportional to the motion speed of the robotic arm.
[0040] In a possible implementation, the window width W is inversely proportional to the end speed v of the robotic arm and is implemented using the following piecewise function:
[0041]
[0042] Among them, W max is the maximum window width, which is applicable when the robotic arm moves at a low speed; W min is the minimum window width, which is applicable when the robotic arm moves at a high speed; v low is the low-speed threshold. When the speed is lower than this speed, the maximum window width W max is used; v high is the high-speed threshold. When the speed is higher than this speed, the minimum window width W min is used.
[0043] In some examples, the deviation α is obtained by real-time calculation of the encoder feedback data of the robotic arm and the forward kinematics model.
[0044] In some examples, a low-pass filtering operation is added when collecting current data to filter the noise of the joint motor.
[0045] In some examples, the first threshold and the second threshold are set based on historical collision data and the real-time load weight, and are dynamically adjusted through an adaptive algorithm.
[0046] In a possible implementation, the specific implementation process of the adaptive threshold dynamic adjustment includes:
[0047] S301: Data collection and feature extraction, record the following parameters under different working conditions, such as: joint rated current I, real-time load weight m, robotic arm motion speed v, etc.
[0048] S302: Threshold prediction model training, which may use a linear regression model to predict a first threshold (current change rate threshold) and a second threshold (trajectory deviation threshold). Other prediction methods may also be used, which are not limited in the present embodiment of the application.
[0049] S303: Online adaptive adjustment.
[0050] In some examples, the method also includes autonomous obstacle avoidance path planning: after the robotic arm stops urgently, the obstacle coordinates are reversely calculated based on the collision position, a local correction trajectory that bypasses the obstacle coordinates is generated, and the operation is automatically resumed.
[0051] In one possible implementation, after the robotic arm collides and stops suddenly, the spatial coordinates of the end effector at the collision point are calculated through inverse kinematics based on the joint angles at the moment of collision, and marked as the center of the obstacle. The safety radius is expanded outward with this coordinate as the center, and two path points before and after the collision point are selected on the original target path as the starting point and end point of the corrected trajectory. The A* algorithm is used to search for a local path that bypasses the safe area in three-dimensional space. After the corrected path is discretized into multiple path points, it is converted into a joint angle sequence, and the robotic arm is controlled to execute at 50% of the rated speed, and the original task is automatically restored after avoiding the obstacle. It should be noted that the embodiments of the present application do not limit the specific local path generation method.
[0052] In some examples, autonomous obstacle avoidance path planning uses inverse kinematics interpolation to generate local correction trajectories.
[0053] In one possible implementation, in the local obstacle avoidance path planning, the starting point (current emergency stop position) and the end point (target position to bypass the obstacle) of the correction trajectory are selected, and the joint angles of the two points are solved respectively by inverse kinematics. A fifth-order polynomial interpolation is used in the joint space to generate a smooth joint angle sequence from the starting point to the end point to ensure the continuity of the velocity and acceleration of each joint. The interpolation formula is:
[0054] θ(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 ,
[0055] Solve the coefficient a by boundary conditions (start and end angles, speed is zero) i (i=1,2,3,4,5), and finally an obstacle avoidance trajectory is generated. When the robot arm moves along this trajectory, the end posture remains stable.
[0056] In some examples, during the generation of the local correction trajectory, a gradient descent method is used for trajectory optimization.
[0057] In a possible implementation, for the preliminarily generated local obstacle avoidance trajectory, the optimization objectives are defined as the shortest path length and the optimal smoothness of joint movement. Taking the coordinates of the discrete trajectory points as variables, a loss function is constructed:
[0058]
[0059] where P i is the coordinate of the path point, θ i is the joint angle, and λ is the weight coefficient. The positions of the path points are iteratively adjusted by the gradient descent method. The partial derivative of the loss function with respect to the path points is calculated, and the coordinates of the path points are updated along the negative gradient direction until the loss function converges. The optimized trajectory reduces the sudden changes of the robotic arm joints while shortening the movement path, improving the obstacle avoidance efficiency.
[0060] The embodiment of the present application provides a control device, which includes a processor and a memory. The processor is coupled to the memory. The memory is used to store computer programs or instructions, and the processor is used to execute the computer programs or instructions in the memory, so that any of the methods in the foregoing embodiments is executed.
[0061] The embodiment of the present application further provides a robotic arm, which includes the control device described above.
[0062] The above is only the preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. Any equivalent modification or change made by those of ordinary skill in the art according to the disclosure of the present invention shall be included in the protection scope recorded in the claims.
Claims
1. A method for warning of robotic arm collisions, characterized in that, The method includes: S1: Collect the current data of each joint motor of the robotic arm in real time, and calculate the current change rate Δi of the current data through a sliding window algorithm; S2: Based on the preset motion trajectory of the robotic arm, collect the actual position information of the end effector of the robotic arm in real time, and calculate the deviation α between the actual position information and the theoretical position information of the end effector; S3: Determine whether the robotic arm needs to issue a collision warning based on the current change rate Δi and the deviation α: If the current change rate Δi of at least one of the joint motors is ≧ the first threshold and the deviation α is ≦ the second threshold, it is determined that the load is abnormal and a speed reduction warning is triggered; If the current change rate Δi of at least one of the joint motors exceeds the first threshold and the deviation α exceeds the second threshold, it is determined that a collision has occurred, trigger an emergency stop and mark the obstacle area.
2. The method according to claim 1, characterized in that, The sliding window algorithm uses a dynamic window width, and the window width is inversely proportional to the movement speed of the robotic arm.
3. The method according to claim 1, wherein The deviation α is obtained by real-time calculation of the encoder feedback data of the robotic arm and the forward kinematics model.
4. The method according to claim 1, characterized in that, Add a low-pass filtering operation when collecting the current data to filter the noise of the joint motor.
5. The method according to claim 1, characterized in that, The setting of the first threshold and the second threshold is based on historical collision data and the real-time load weight, and is dynamically adjusted through an adaptive algorithm.
6. The method according to claim 1, characterized in that The method further includes autonomous obstacle avoidance path planning: After the robotic arm stops emergently, the obstacle coordinates are reversely calculated based on the collision position, a local correction trajectory that bypasses the obstacle coordinates is generated, and the operation is automatically resumed.
7. The method according to claim 6, wherein The autonomous obstacle avoidance path planning uses inverse kinematics interpolation to generate the local correction trajectory.
8. The method according to claim 6, characterized in that, During the generation of the local correction trajectory, the gradient descent method is used for trajectory optimization.
9. A control device, characterized in that, It includes a processor and a memory, the processor is coupled with the memory, the memory is used to store computer programs or instructions, and the processor is used to execute the computer programs or instructions in the memory, so that the method according to any one of claims 1 to 8 is executed.
10. A robotic arm, characterized in that, The robotic arm includes the control device according to claim 9.