A robot protection control method and device based on motion state detection

Abstract

The present invention discloses a robot protection control method and device based on motion state detection, which relates to the field of robot control technology, including obtaining motion state data and external collision data, evaluating the imbalance state, adjusting the joint motor to lower the center of gravity if imbalanced, re-evaluating until balanced, obtaining robot vibration data after balancing, evaluating avoidance requirements, and using avoidance algorithms to perform avoidance when avoidance is required until avoidance is no longer required, and then continuing to act. The present invention obtains motion state data and external collision data, and evaluates according to the posture abnormality coefficient and the collision imbalance coefficient to determine whether the robot is unbalanced, which helps to improve the accuracy of the robot imbalance judgment, adjusts the joint angle of the robot through the joint motor, shifts the center of gravity of the robot, and rebalances the robot, and lowers the center of gravity to reduce the torque of the robot, thereby improving the stability of the robot, thereby improving the balance of the robot and improving its action efficiency.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and in particular to a robot protection control method and device based on motion state detection. Background Art

[0002] Robot protection control is a comprehensive set of technologies and strategies. Its core purpose is to ensure that the robot can maximize the safety of operators and prevent possible damage to the surrounding environment when performing various complex tasks, while also protecting the robot's own structure and function from damage. This system not only involves the robot's physical protection, such as emergency stop buttons, safety fences, etc., but also includes advanced sensor technology, software algorithms and communication protocols. These technologies can realize real-time monitoring and control of robot behavior to deal with potential risks and abnormal situations.

[0003] In the application scenarios of modern robotics technology, robots often need to perform tasks in complex and changing environments, which requires them to have a high degree of autonomous obstacle avoidance capabilities. When faced with moving obstacles, robots must be able to quickly identify potential collision risks and adopt flexible obstacle avoidance strategies to ensure mission continuity and their own safety.

[0004] For example, the invention patent with publication number: CN115816470B discloses a quadruped robot motion protection method and system, which includes the following steps: S1. Acquire the real position of the joint motor based on the encoder of the joint motor, and generate the real torque according to the real current of the joint motor; S2. Detect whether the quadruped robot is in an abnormal motion state according to the roll angle information, pitch angle information, force information, real position, real torque, expected position and expected torque, and acquire the type of abnormal motion state when the quadruped robot is in an abnormal motion state, and the abnormal motion state includes an unbalanced state, a foot end obstructed state and an overturning state; S3. When it is detected that the quadruped robot is in an abnormal motion state, control the joint motor to execute the corresponding preset instruction according to the type of the abnormal motion state, and the preset instruction is used to eliminate the corresponding type of abnormal motion state; the method can automatically detect and eliminate the abnormal motion state of the quadruped robot.

[0005] For example, the invention patent with publication number: CN114102592B discloses a robot protection control method, device and equipment based on motion state detection, including: first obtaining the robot's whole machine current value, the state of each servo of the robot and the current posture of the robot, if the robot's whole machine current value is greater than or equal to the preset current threshold, and the state of each servo of the robot is normal, then according to the current posture of the robot, control the robot to perform the target action. This application can realize the detection of the robot's motion state, avoid abnormal motion causing excessive whole machine current value, and cause damage to the robot.

[0006] When the robot is performing a task and choosing the best path to move, if it encounters a moving obstacle, it will cause the robot to lose balance and reduce its efficiency of movement. If it encounters a moving obstacle, it may knock the robot down or loosen its body parts in serious cases, affecting the robot's own safety and delaying the progress of the task. Summary of the invention

[0007] Technical issues solved

[0008] In view of the deficiencies in the prior art, the present invention provides a robot protection control method and device based on motion state detection, which solves the problems of the robot being unbalanced due to collision and the robot being unsafe due to collision.

[0009] Technical Solution

[0010] To achieve the above purpose, the present invention is implemented through the following technical solutions: a robot protection control method and device based on motion state detection, including the following specific steps: step one: obtaining motion state data and external collision data, and preprocessing the motion state data and external collision data; evaluating the preprocessed motion state data and external collision data to obtain a robot imbalance evaluation value; step two: judging the robot imbalance evaluation value; step three: if it is judged that the robot is unbalanced, adjusting the joint motor, and lowering the center of gravity of the robot in a balanced state, and returning to step one to continue obtaining motion state data and external collision data until it is judged that the robot is balanced; if it is judged that the robot is balanced, executing step four; step four: setting a third pressure sensor to obtain robot vibration data;

[0011] Preprocess the robot vibration data, evaluate the preprocessed robot vibration data, and obtain the robot avoidance evaluation value; Step five: judge the robot avoidance evaluation value; Step six: if it is judged that the robot needs to avoid, set the positioning sensor and use the path avoidance algorithm to avoid, and return to step four to continue to obtain the robot vibration data until it is judged that the robot does not need to avoid; if it is judged that the robot does not need to avoid, the robot continues to move.

[0012] Further, in step one, a monitoring time period is set and divided into several sub-time periods; the motion state data includes the number of supports, the support force, the center of gravity position of the robot and the center position of the robot; the external collision data includes the collision pressure direction and the collision time; the motion state data and the external collision data are filtered and denoised; the motion state data and the external collision data after the filtering and denoising process are comprehensively analyzed to obtain the posture abnormality coefficient and the collision imbalance coefficient; the posture abnormality coefficient and the collision imbalance coefficient are normalized; the normalized posture abnormality coefficient and the collision imbalance coefficient are comprehensively analyzed to obtain the robot imbalance assessment value; ;

[0013] in, represents the robot imbalance assessment value, represents the coefficient of posture abnormality, Represents the collision imbalance coefficient.

[0014] Furthermore, the specific method for obtaining the posture abnormality coefficient is as follows: summing the support forces to obtain the sum of the first support forces; averaging the sum of the first support forces to obtain the first average support force; subtracting the first average support forces from each other according to the number of support members, and taking the absolute value of the calculation result to obtain the first balance difference value; summing the first balance difference values ​​to obtain the sum of the first balance difference values; summing the sum of the first balance difference values ​​to obtain the sum of the second balance difference values; setting the standard bearing capacity; subtracting the sum of the second balance difference values ​​from the standard bearing capacity, and taking the absolute value of the calculation result to obtain to the support force imbalance value; set a three-dimensional coordinate system with x-axis, y-axis and z-axis; introduce the robot's center of gravity position and the robot's center position to obtain the robot's center of gravity position coordinates and the robot's center position coordinates; calculate the robot's center position coordinates and the robot's center of gravity position coordinates using the Euclidean distance formula to obtain the center of gravity deviation distance; set the center of gravity deviation distance threshold; compare the center of gravity deviation distance with the center of gravity deviation distance threshold; if the center of gravity deviation distance is greater than the center of gravity deviation distance threshold, then calculate the difference between the center of gravity deviation distance and the limit deviation distance to obtain the center of gravity imbalance distance value; compare the robot's center position coordinates and the robot's center of gravity position coordinates The vector dot product method is used to calculate the center of gravity deflection angle; the threshold of the center of gravity deflection angle is set; according to the pitch angle interval around the y axis, the yaw angle interval around the z axis and the roll angle interval around the x axis, the center of gravity deflection angle is divided into the Y angle around the y axis, the Z angle around the z axis and the X angle around the x axis respectively; the X angle is matched with the roll angle interval around the x axis, if it does not match, the X angle is calculated to obtain the roll angle difference value with the endpoint value of the roll angle interval closer to the X angle; the absolute value of the roll angle difference value is taken to obtain the roll angle imbalance value; the Y angle is matched with the pitch angle interval around the y axis, if it does not match, the Y angle is matched The pitch angle interval endpoint value that is closer to the Y angle is calculated by difference calculation to obtain the pitch angle difference value; the absolute value of the pitch angle difference value is taken to obtain the pitch angle imbalance value; the Z angle is matched with the yaw angle interval around the z-axis, and if it does not match, the Z angle is calculated by difference calculation to obtain the yaw angle difference value; the absolute value of the yaw angle difference value is taken to obtain the yaw angle imbalance value; the roll angle imbalance value, the pitch angle imbalance value and the yaw angle imbalance value are summed to obtain the center of gravity imbalance angle value; the support force imbalance value, the center of gravity imbalance distance value and the center of gravity imbalance angle value are comprehensively analyzed to obtain the attitude abnormality coefficient.

[0015] Furthermore, the specific method of obtaining the center of gravity deviation distance is as follows: Set the robot center position coordinates to , the robot's center of gravity coordinates are ; ,in Indicates the distance of center of gravity deviation. The x-axis coordinate of the robot's center of gravity. The y-axis coordinate of the robot's center of gravity. The z-axis coordinate of the robot's center of gravity.

[0016] Furthermore, the specific method for obtaining the center of gravity deflection angle is as follows: set three unit vectors to obtain a unit roll angle vector of the x-axis, a unit pitch angle vector of the y-axis, and a unit yaw angle vector of the z-axis; set the center of gravity deflection vector; perform dot product calculation on the center of gravity deflection vector and the unit roll angle vector of the x-axis to obtain the X angle; perform dot product calculation on the center of gravity deflection vector and the unit pitch angle vector of the y-axis to obtain the Y angle; perform dot product calculation on the center of gravity deflection vector and the unit yaw angle vector of the z-axis to obtain the Z angle; combine the X angle, the Y angle, and the Z angle to obtain the center of gravity deflection angle.

[0017] Furthermore, in step 2, a robot imbalance threshold is set and compared with the robot imbalance assessment value; if the robot imbalance assessment value is greater than or equal to the robot imbalance threshold, the robot is judged to be unbalanced; if the robot imbalance assessment value is less than the robot imbalance threshold, the robot is judged to be balanced.

[0018] Further, in step 4, the robot vibration data includes vibration intensity and vibration frequency; filtering and denoising the robot vibration data; comprehensively analyzing the robot vibration data after filtering and denoising to obtain a safety anomaly coefficient; normalizing the safety anomaly coefficient, posture anomaly coefficient and collision imbalance coefficient; comprehensively analyzing the safety anomaly coefficient, posture anomaly coefficient and collision imbalance coefficient after normalization to obtain a robot avoidance evaluation value; ;in, represents the robot avoidance evaluation value, represents the safety anomaly coefficient, represents the coefficient of posture abnormality, Represents the collision imbalance coefficient.

[0019] Furthermore, the specific method for obtaining the safety anomaly coefficient is as follows: summing up the vibration intensities to obtain the sum of the vibration intensities; averaging the sum of the vibration intensities to obtain a first average vibration intensity; summing up several first average vibration intensities to obtain the sum of the vibration intensities within the monitoring time period; averaging the sum of the vibration intensities within the monitoring time period to obtain a second average vibration intensity; calculating the variance of several first average vibration intensities and the second average vibration intensities to obtain a vibration intensity fluctuation value; summing up the vibration frequencies to obtain the sum of the vibration frequencies; averaging the sum of the vibration frequencies to obtain the average vibration frequency; calculating the variance of several vibration frequencies and the average vibration frequency to obtain a vibration frequency fluctuation value; and comprehensively analyzing the vibration intensity fluctuation value and the vibration frequency fluctuation value to obtain the safety anomaly coefficient.

[0020] Furthermore, in step five, a robot avoidance threshold is set and compared with the robot avoidance evaluation value; if the robot avoidance evaluation value is greater than or equal to the robot avoidance threshold, it is determined that the robot needs to avoid; if the robot avoidance evaluation value is less than the robot avoidance threshold, it is determined that the robot does not need to avoid.

[0021] Furthermore, a data acquisition module, a data analysis module, a data execution module, a joint motor module and a positioning sensor module; the data acquisition module is used to acquire motion state data, external collision data and robot vibration data and pre-process them, and send the pre-processed data to the data analysis module; the data analysis module is used to receive the data sent by the data acquisition module, and analyze and judge the pre-processed motion state data, external collision data and robot vibration data, and send the results of the analysis and judgment to the data execution module; the data execution module is used to receive the results of the analysis and judgment of the data analysis module, and adjust the results of the imbalance judgment and avoidance judgment of the robot, and send the adjustment instructions to the joint motor module and the positioning sensor module; the joint motor module is used to receive the adjustment instructions of the data execution module, adjust the joint angles of the robot and lower the center of gravity of the robot; the positioning sensor module is used to receive the adjustment instructions of the data execution module, and avoid according to the acquired real-time position data and the path avoidance algorithm.

[0022] Beneficial Effects

[0023] Compared with the prior art, the embodiments of the present invention have at least the following advantages or beneficial effects:

[0024] 1. By obtaining motion state data and external collision data, and evaluating based on the posture abnormality coefficient and collision imbalance coefficient, it is used to determine whether the robot is unbalanced, which helps to improve the accuracy of robot imbalance judgment.

[0025] 2. By obtaining the robot's vibration data and evaluating it based on the safety abnormality coefficient, posture abnormality coefficient, and collision imbalance coefficient, it is used to determine whether the robot needs to avoid an obstacle, which helps to improve the accuracy of the robot's avoidance judgment.

[0026] 3. The joint angles of the robot are adjusted through the joint motors to shift the center of gravity of the robot and rebalance the robot. The center of gravity is lowered to reduce the torque of the robot, thereby improving the stability of the robot, thereby improving the balance of the robot and its efficiency of movement.

[0027] 4. By setting up positioning sensors and using path avoidance algorithms to avoid obstacles, it helps protect the robot from safety impacts.

[0028] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 The present invention is a flow chart of a robot protection control method based on motion state detection.

[0030] Figure 2 This is a line graph of the influence of external pressure on the balance of the robot.

[0031] Figure 3 This is the present invention: a structural diagram of a robot protection control device based on motion state detection. DETAILED DESCRIPTION

[0032] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0033] It should be noted that, in this article, relational terms such as first and second are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0034] like Figure 1As shown, an embodiment of the present invention provides a robot protection control method based on motion state detection, which includes the following specific steps:

[0035] Step 1: Setting a first pressure sensor, a second pressure sensor and a center of gravity sensor, obtaining motion state data through the first pressure sensor and the center of gravity sensor, and obtaining external collision data through the second pressure sensor;

[0036] Set the monitoring time period and divide it into several sub-time periods;

[0037] The motion state data includes the number of supports, support force, robot center of gravity position, and robot center position;

[0038] Number of supports: indicates the number of supports that support the balance of the robot;

[0039] Support force: represents a number of support force values ​​of each support member of the robot in each sub-time period obtained by the first pressure sensor;

[0040] Robot center of gravity position: indicates the robot's center of gravity position during the monitoring period obtained by the center of gravity sensor;

[0041] Robot center position: indicates the center point of the robot body. This center point is the anchor point of the robot's center of gravity and also the origin of the three-dimensional coordinate system.

[0042] In the robot's balanced state, the robot's center of gravity position coincides with the robot's center position;

[0043] External collision data include collision pressure direction and collision moment;

[0044] Collision pressure direction: indicates the pressure direction fed back by a single external collision of the robot during the monitoring period;

[0045] Collision time: indicates the initial and final time when the robot is subjected to external collision during the monitoring period;

[0046] Initial moment: indicates the moment when the robot is subjected to external collision and obtains collision pressure;

[0047] End time: indicates the time when the robot is freed from the collision pressure due to external collision;

[0048] Preprocessing the motion state data and the external collision data, evaluating the preprocessed motion state data and the external collision data, and obtaining the robot imbalance evaluation value;

[0049] Filtering and denoising motion state data and external collision data helps improve data quality;

[0050] Comprehensively analyze the motion state data and external collision data after filtering and noise reduction to obtain the posture abnormality coefficient and collision imbalance coefficient;

[0051] Normalizing the posture anomaly coefficient and collision imbalance coefficient helps improve data processing efficiency;

[0052] Comprehensively analyze the normalized posture abnormality coefficient and collision imbalance coefficient to obtain the robot imbalance assessment value;

[0053] ;

[0054] in, represents the robot imbalance assessment value, represents the coefficient of posture abnormality, represents the collision imbalance coefficient;

[0055] By obtaining motion state data and external collision data, and evaluating based on the posture abnormality coefficient and collision imbalance coefficient, it is used to determine whether the robot is unbalanced, which helps to improve the accuracy of robot imbalance judgment.

[0056] The specific method of obtaining the posture abnormality coefficient is as follows:

[0057] The support forces are summed up to obtain the sum of the first support forces;

[0058] The sum of the first supporting force: represents the sum of several supporting force values ​​of each supporting part of the robot in each sub-time period;

[0059] Calculate the average of the sum of the first supporting forces to obtain a first average supporting force;

[0060] The first average support force: represents the average support force value obtained according to the number of support members in each sub-time period;

[0061] The first average supporting forces are calculated based on the number of supporting members, and the absolute value of the calculation result is taken to obtain a first balance difference value;

[0062] Performing a sum calculation on the first balance difference values ​​to obtain a sum of the first balance difference values;

[0063] The sum of the first balance difference values: represents the sum of the first balance difference values ​​obtained by the robot according to the number of support members in each sub-time period;

[0064] The sum of the first balance difference values ​​is calculated to obtain the sum of the second balance difference values;

[0065] The sum of the second balance difference values: represents the sum of the first balance difference values ​​obtained according to the number of support members during the monitoring period;

[0066] Setting standard bearing capacity;

[0067] Standard bearing capacity: represents the sum of the standard first balance difference values ​​obtained according to the number of supports during the monitoring period;

[0068] The difference between the sum of the second balance difference value and the standard bearing capacity is calculated, and the absolute value of the calculation result is taken to obtain the support force imbalance value;

[0069] Set the three-dimensional coordinate system, that is, the coordinate system of the x-axis, y-axis and z-axis;

[0070] The robot's center of gravity position and the robot's center position are introduced to obtain the robot's center of gravity position coordinates and the robot's center position coordinates;

[0071] Robot center position coordinates: represents the origin coordinates;

[0072] Robot center of gravity position coordinates: indicates the robot center of gravity position coordinates obtained in real time according to the robot's motion state data during the monitoring period;

[0073] The robot center position coordinates and the robot gravity center position coordinates are calculated using the Euclidean distance formula to obtain the gravity center deviation distance;

[0074] Euclidean distance formula: used to calculate the deviation distance of the robot's center of gravity position coordinates from the robot's center position coordinates in three-dimensional space;

[0075] Set the center of gravity deviation distance threshold;

[0076] Center of gravity deviation distance threshold: indicates the maximum deviation distance of the center of gravity of the robot when it is in a balanced state;

[0077] Obtain historical experimental data of the limit deviation distance of the center of gravity through motion state data;

[0078] The historical experimental data of the limit deviation distance of the center of gravity are stored in the database of the limit deviation distance of the center of gravity;

[0079] The gravity center deviation distance threshold calls the gravity center limit deviation distance historical experimental data of the gravity center limit deviation distance database;

[0080] Compare the center of gravity deviation distance with the center of gravity deviation distance threshold;

[0081] If the center of gravity deviation distance is greater than the center of gravity deviation distance threshold, the center of gravity deviation distance and the limit deviation distance are calculated to obtain the center of gravity imbalance distance value;

[0082] The robot center position coordinates and the robot gravity center position coordinates are calculated by vector dot product method to obtain the gravity center deflection angle;

[0083] Vector dot product method: used to calculate the deflection angle of the robot's center of gravity position coordinates to the robot's center position coordinates in three-dimensional space;

[0084] Set the center of gravity deflection angle threshold;

[0085] Center of gravity deflection angle threshold: indicates the deflection angle of the center of gravity of the robot in a balanced state. This angle is divided into three angle ranges, namely the pitch angle range around the y-axis, the yaw angle range around the z-axis, and the roll angle range around the x-axis.

[0086] The endpoints of the range of the three angles: indicate the limit angle of the center of gravity deflection of the robot in a balanced state;

[0087] Obtain historical experimental data of the limit deflection angle of the center of gravity through motion state data;

[0088] The historical experimental data of the limit deflection angle of the center of gravity are stored in the database of the limit deflection angle of the center of gravity;

[0089] The gravity center deflection angle threshold value calls the gravity center limit deflection angle historical experimental data of the gravity center limit deflection angle database;

[0090] According to the pitch angle interval around the y-axis, the yaw angle interval around the z-axis, and the roll angle interval around the x-axis, the center of gravity deflection angle is divided into Y angles around the y-axis, Z angles around the z-axis, and X angles around the x-axis respectively;

[0091] Match the X angle with the roll angle interval around the x-axis. If they do not match, calculate the difference between the X angle and the endpoint value of the roll angle interval that is closer to the X angle to obtain the roll angle difference value.

[0092] Take the absolute value of the roll angle difference to obtain the roll angle imbalance value;

[0093] Match the Y angle with the pitch angle interval around the Y axis. If they do not match, calculate the difference between the Y angle and the endpoint value of the pitch angle interval that is closer to the Y angle to obtain the pitch angle difference value.

[0094] Take the absolute value of the pitch angle difference to obtain the pitch angle imbalance value;

[0095] Match the Z angle with the yaw angle interval around the z-axis. If they do not match, calculate the difference between the Z angle and the endpoint value of the yaw angle interval that is closer to the Z angle to obtain the yaw angle difference value;

[0096] Take the absolute value of the yaw angle difference to obtain the yaw angle imbalance value;

[0097] The roll angle imbalance value, the pitch angle imbalance value and the yaw angle imbalance value are summed up to obtain the center of gravity imbalance angle value;

[0098] Comprehensively analyze the support force imbalance value, center of gravity imbalance distance value and center of gravity imbalance angle value to obtain the posture abnormality coefficient;

[0099] ;

[0100] in, represents the coefficient of posture abnormality, Indicates the support force imbalance value, Indicates the center of gravity imbalance distance value, Indicates the value of the center of gravity imbalance angle.

[0101] The specific method of obtaining the center of gravity deviation distance is as follows:

[0102] Set the robot center position coordinates to , the robot's center of gravity coordinates are ;

[0103] ,in Indicates the distance of center of gravity deviation. The x-axis coordinate of the robot's center of gravity. The y-axis coordinate of the robot's center of gravity. The z-axis coordinate of the robot's center of gravity.

[0104] The specific method of obtaining the center of gravity deflection angle is as follows:

[0105] Set three unit vectors to get the unit roll angle vector of the x-axis, the unit pitch angle vector of the y-axis, and the unit yaw angle vector of the z-axis;

[0106] Set the center of gravity deviation vector;

[0107] Center of gravity deflection vector: indicates the deflection vector of the robot's center of gravity;

[0108] The dot product of the center of gravity deflection vector and the unit roll angle vector of the x-axis is calculated to obtain the X angle. ,in represents the X angle, Indicates the gravity center deviation momentum, represents the unit roll angle vector of the x-axis, The modulus of the gravity center deviation momentum;

[0109] Calculate the dot product of the center of gravity deflection vector and the unit pitch angle vector of the y-axis to get the Y angle, ,in represents the Y angle, Indicates the gravity center deviation momentum, represents the unit pitch angle vector of the y-axis, The modulus of the gravity center deviation momentum;

[0110] The dot product of the center of gravity deflection vector and the unit yaw angle vector of the z-axis is calculated to obtain the Z angle. ;in represents the Z angle, Indicates the gravity center deviation momentum, represents the unit yaw angle vector of the z-axis, The modulus of the gravity center deviation momentum;

[0111] The X angle, Y angle and Z angle are combined to obtain the center of gravity deflection angle.

[0112] The specific method of obtaining the collision imbalance coefficient is as follows:

[0113] Several collision pressure directions are converted into vector forms to obtain several collision pressure vectors;

[0114] According to the vector property of force, several collision pressure vectors are summed and calculated to obtain the collision resultant force vector;

[0115] The dot product of the center of gravity deflection vector and the collision force vector is calculated to obtain the collision impact value. ,in Indicates the collision impact value. When the gravity center deflection vector is in the same direction as the collision resultant force vector, that is, the robot is unbalanced due to external pressure, the angle between the gravity center deflection vector and the collision resultant force vector is "0", which means is "1", , Indicates the gravity center deviation momentum, represents the collision force vector, The modulus of the gravity center deviation momentum, Represents the magnitude of the collision force vector;

[0116] Comprehensively analyze the collision impact value and posture abnormality coefficient to obtain the collision imbalance coefficient;

[0117] ;

[0118] in, represents the collision imbalance coefficient, represents the coefficient of posture abnormality, Represents the collision impact value, represents a real number, so When it is "0", it will not be affected and .

[0119] Step 2: Determine the robot imbalance assessment value;

[0120] Set the robot imbalance threshold and compare it with the robot imbalance assessment value;

[0121] Obtain robot imbalance historical experimental data through motion state data and external collision data;

[0122] Store the robot imbalance historical experimental data into the robot imbalance database;

[0123] The robot imbalance threshold calls the robot imbalance historical experimental data in the robot imbalance database;

[0124] If the robot imbalance assessment value is greater than or equal to the robot imbalance threshold, the robot is judged to be unbalanced;

[0125] If the robot imbalance assessment value is less than the robot imbalance threshold, the robot is judged to be balanced.

[0126] Step 3: If the robot is judged to be unbalanced, the joint motors are adjusted, and the center of gravity of the robot in a balanced state is lowered, and the process returns to step 1 to continue acquiring motion state data and external collision data until the robot is judged to be balanced;

[0127] like Figure 2 As shown: the joint angles of the robot are adjusted through the joint motors to shift the center of gravity of the robot, shorten the distance between the center of gravity and the ground, and thus rebalance the robot. Lowering the center of gravity reduces the torque of the robot, improves the stability of the robot, and thus improves the balance of the robot and its action efficiency.

[0128] If the robot is judged to be balanced, execute step 4;

[0129] Table 1 The influence of external pressure on robot balance

[0130]

[0131] As shown in Table 1, in the first set of data, when the external pressure is 0.3N, the center of gravity deviation distance is 1.2CM; in the second set of data, when the external pressure is 1.4N, the center of gravity deviation distance is 3.4CM; in the third set of data, when the external pressure is 2.7N, the center of gravity deviation distance is 5.1CM;

[0132] As the external pressure on the robot increases, the center of gravity deviates further, which means that the robot's balance becomes worse. It is necessary to adjust the joint motors and lower the center of gravity in time to shorten the distance between the center of gravity and the ground in order to improve the robot's balance.

[0133] Step 4: Set up the third pressure sensor to obtain robot vibration data;

[0134] The robot vibration data includes vibration intensity and vibration frequency;

[0135] Vibration intensity: indicates the intensity of vibrations to which the robot is subjected in each sub-time period;

[0136] Vibration frequency: indicates the frequency of vibration to which the robot is subjected in each sub-time period;

[0137] Preprocessing the robot vibration data, evaluating the preprocessed robot vibration data, and obtaining a robot avoidance evaluation value;

[0138] Filtering and denoising the robot vibration data helps improve the data quality;

[0139] Comprehensively analyze the robot vibration data after filtering and noise reduction to obtain the safety anomaly coefficient;

[0140] Normalizing the safety anomaly coefficient, posture anomaly coefficient, and collision imbalance coefficient helps improve calculation efficiency;

[0141] Comprehensively analyze the normalized safety anomaly coefficient, posture anomaly coefficient, and collision imbalance coefficient to obtain the robot avoidance evaluation value;

[0142] ;

[0143] in, represents the robot avoidance evaluation value, represents the safety anomaly coefficient, represents the coefficient of posture abnormality, represents the collision imbalance coefficient;

[0144] By obtaining the robot's vibration data and evaluating it based on the safety abnormality coefficient, posture abnormality coefficient, and collision imbalance coefficient, it can be used to determine whether the robot needs to avoid, which helps to improve the accuracy of the robot's avoidance judgment.

[0145] The specific method of obtaining the safety anomaly coefficient is as follows:

[0146] The vibration intensities are summed and calculated to obtain the sum of the vibration intensities;

[0147] The sum of the vibration intensities is averaged to obtain a first average vibration intensity;

[0148] Calculating and summing a plurality of first average vibration intensities to obtain a sum of the vibration intensities within a monitoring time period;

[0149] The sum of the vibration intensities within the monitoring time period is averaged to obtain a second average vibration intensity;

[0150] Performing variance calculation on a number of first average vibration intensities and second average vibration intensities to obtain a vibration intensity fluctuation value;

[0151] The vibration frequencies are summed up to obtain the sum of the vibration frequencies;

[0152] The sum of the vibration frequencies is averaged to obtain the average vibration frequency;

[0153] The variance of several vibration frequencies and the average vibration frequency is calculated to obtain the vibration frequency fluctuation value;

[0154] Comprehensively analyze the vibration intensity fluctuation value and the vibration frequency fluctuation value to obtain the safety abnormality coefficient;

[0155] ;

[0156] in, represents the safety anomaly coefficient, Indicates the vibration intensity fluctuation value, Indicates the vibration frequency fluctuation value.

[0157] Step 5: Determine the robot's avoidance evaluation value;

[0158] Set the robot avoidance threshold and compare it with the robot avoidance evaluation value;

[0159] Obtain robot avoidance historical experimental data through robot vibration data;

[0160] The robot avoidance historical experimental data is stored in the robot avoidance database;

[0161] The robot avoidance threshold calls the robot avoidance historical experimental data in the robot avoidance database;

[0162] If the robot avoidance evaluation value is greater than or equal to the robot avoidance threshold, it is determined that the robot needs to avoid;

[0163] If the robot avoidance evaluation value is less than the robot avoidance threshold, it is determined that the robot does not need to avoid.

[0164] Step 6: If it is determined that the robot needs to avoid, then set the positioning sensor and use the path avoidance algorithm to avoid, and return to step 4 to continue to obtain the robot vibration data until it is determined that the robot does not need to avoid;

[0165] If it is determined that the robot does not need to avoid, the robot continues to move.

[0166] The specific method of avoiding using the path avoidance algorithm is as follows:

[0167] Obtain the robot's real-time position data through positioning sensors;

[0168] When the robot determines that it needs to avoid, it counts the number of times the robot is unbalanced and vibrated, and traverses to the corresponding position according to the counted number of times of unbalanced and vibrated to obtain several suffering position values;

[0169] Comprehensively analyze several suffering position values ​​to obtain the suffering center position value;

[0170] According to the suffering center position value, a two-dimensional coordinate of a bird's-eye view is established for the suffering position value, and a coordinate reference origin and a number of suffering position coordinates distributed according to coordinate quadrants are obtained;

[0171] A number of crucifixion position coordinates distributed in coordinate quadrants are marked as crucifixion position coordinates;

[0172] The Euclid distance formula is used to calculate the coordinates of the suffering position and the coordinate reference origin, and the distances of the coordinates of several suffering positions in each quadrant to the coordinate reference origin are obtained;

[0173] The distances of the coordinates of several suffering positions in each quadrant to the coordinate reference origin are recorded as experimental distances;

[0174] Arrange the experimental distances in order from large to small to obtain the coordinates of the suffering position farthest from the coordinate reference origin in each quadrant;

[0175] The coordinates of the crucifixion position farthest from the coordinate reference origin in each quadrant are marked as valid coordinates;

[0176] Calculate the effective coordinates of the first quadrant and the effective coordinates of the second quadrant using the Euclidean distance formula to obtain the first side length;

[0177] The effective coordinates of the second quadrant and the effective coordinates of the third quadrant are calculated using the Euclidean distance formula to obtain the second side length;

[0178] Calculate the effective coordinates of the third quadrant and the effective coordinates of the fourth quadrant using the Euclidean distance formula to obtain the third side length;

[0179] The effective coordinates of the fourth quadrant are calculated with the effective coordinates of the first quadrant using the Euclidean distance formula to obtain the fourth side length;

[0180] Connect the first side length, the second side length, the third side length and the fourth side length to obtain the suffering range;

[0181] Comprehensively analyze the suffering range to obtain the suffering area;

[0182] The robot moves in a path that is not within the affected area according to the real-time position data to avoid the situation, which helps protect the robot from safety impacts.

[0183] like Figure 3As shown: A robot protection control device based on motion state detection, comprising: a data acquisition module, a data analysis module, a data execution module, a joint motor module and a positioning sensor module;

[0184] The data acquisition module is used to acquire motion state data, external collision data and robot vibration data and pre-process them, and send the pre-processed data to the data analysis module;

[0185] The data analysis module is used to receive the data sent by the data acquisition module, analyze and judge the pre-processed motion state data, external collision data and robot vibration data, and send the results of the analysis and judgment to the data execution module;

[0186] The data execution module is used to receive the results of the analysis and judgment of the data analysis module, adjust the results of the imbalance judgment and avoidance judgment of the robot, and send the adjustment instructions to the joint motor module and the positioning sensor module;

[0187] The joint motor module is used to receive the adjustment instructions from the data execution module, adjust the joint angles of the robot and lower the center of gravity of the robot;

[0188] The positioning sensor module is used to receive the adjustment instructions of the data execution module and perform avoidance according to the acquired real-time position data and the path avoidance algorithm.

[0189] The preferred embodiments of the present invention disclosed above are only used to help explain the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific implementation methods described. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A robot protection control method based on motion state detection, characterized in that: The specific steps include: Step 1: Acquire motion state data and external collision data, and pre-process the motion state data and external collision data; The preprocessed motion state data and external collision data are evaluated to obtain the robot imbalance evaluation value; Step 2: Determine the robot imbalance assessment value; Step 3: If the robot is judged to be unbalanced, the joint motors are adjusted, and the center of gravity of the robot in a balanced state is lowered, and the process returns to step 1 to continue acquiring motion state data and external collision data until the robot is judged to be balanced; If the robot is judged to be balanced, execute step 4; Step 4: Set up the third pressure sensor to obtain robot vibration data; Preprocessing the robot vibration data, evaluating the preprocessed robot vibration data, and obtaining a robot avoidance evaluation value; Step 5: Determine the robot's avoidance evaluation value; Step 6: If it is determined that the robot needs to avoid, then set the positioning sensor and use the path avoidance algorithm to avoid, and return to step 4 to continue to obtain the robot vibration data until it is determined that the robot does not need to avoid; If it is determined that the robot does not need to avoid, the robot continues to move; In step 1, a monitoring time period is set and divided into several sub-time periods; The motion state data includes the number of supports, the support force, the robot's center of gravity position, and the robot's center position; The external collision data includes the collision pressure direction and the collision time; Perform filtering and noise reduction on motion state data and external collision data; Comprehensively analyze the motion state data and external collision data after filtering and noise reduction to obtain the posture abnormality coefficient and collision imbalance coefficient; Normalize the posture abnormality coefficient and collision imbalance coefficient; Comprehensively analyze the normalized posture abnormality coefficient and collision imbalance coefficient to obtain the robot imbalance assessment value; ； in, represents the robot imbalance assessment value, represents the coefficient of posture abnormality, represents the collision imbalance coefficient; In step 4, the robot vibration data includes vibration intensity and vibration frequency; Filter and reduce noise on robot vibration data; Comprehensively analyze the robot vibration data after filtering and noise reduction to obtain the safety anomaly coefficient; Normalize the safety anomaly coefficient, posture anomaly coefficient, and collision imbalance coefficient; Comprehensively analyze the normalized safety anomaly coefficient, posture anomaly coefficient, and collision imbalance coefficient to obtain the robot avoidance evaluation value; ； in, represents the robot avoidance evaluation value, represents the safety anomaly coefficient, represents the coefficient of posture abnormality, Represents the collision imbalance coefficient.

2. A robot protection control method based on motion state detection according to claim 1, characterized in that: The specific method for obtaining the posture abnormality coefficient is as follows: The support forces are summed up to obtain the sum of the first support forces; Calculate the average of the sum of the first supporting forces to obtain a first average supporting force; The first average supporting forces are calculated based on the number of supporting members, and the absolute value of the calculation result is taken to obtain a first balance difference value; Performing a sum calculation on the first balance difference values ​​to obtain a sum of the first balance difference values; The sum of the first balance difference values ​​is calculated to obtain the sum of the second balance difference values; Setting standard bearing capacity; The difference between the sum of the second balance difference value and the standard bearing capacity is calculated, and the absolute value of the calculation result is taken to obtain the support force imbalance value; Setting a three-dimensional coordinate system having an x-axis, a y-axis, and a z-axis; The robot's center of gravity position and the robot's center position are introduced to obtain the robot's center of gravity position coordinates and the robot's center position coordinates; The robot center position coordinates and the robot gravity center position coordinates are calculated using the Euclidean distance formula to obtain the gravity center deviation distance; Set the center of gravity deviation distance threshold; Compare the center of gravity deviation distance with the center of gravity deviation distance threshold; If the center of gravity deviation distance is greater than the center of gravity deviation distance threshold, the center of gravity deviation distance and the limit deviation distance are calculated to obtain the center of gravity imbalance distance value; The robot center position coordinates and the robot gravity center position coordinates are calculated by vector dot product method to obtain the gravity center deflection angle; Set the center of gravity deflection angle threshold; According to the pitch angle interval around the y-axis, the yaw angle interval around the z-axis, and the roll angle interval around the x-axis, the center of gravity deflection angle is divided into Y angles around the y-axis, Z angles around the z-axis, and X angles around the x-axis respectively; Match the X angle with the roll angle interval around the x-axis. If they do not match, calculate the difference between the X angle and the endpoint value of the roll angle interval that is closer to the X angle to obtain the roll angle difference value. Take the absolute value of the roll angle difference to obtain the roll angle imbalance value; Match the Y angle with the pitch angle interval around the Y axis. If they do not match, calculate the difference between the Y angle and the endpoint value of the pitch angle interval that is closer to the Y angle to obtain the pitch angle difference value. Take the absolute value of the pitch angle difference to obtain the pitch angle imbalance value; Match the Z angle with the yaw angle interval around the z-axis. If they do not match, calculate the difference between the Z angle and the endpoint value of the yaw angle interval that is closer to the Z angle to obtain the yaw angle difference value; Take the absolute value of the yaw angle difference to obtain the yaw angle imbalance value; The roll angle imbalance value, the pitch angle imbalance value and the yaw angle imbalance value are summed up to obtain the center of gravity imbalance angle value; The support force imbalance value, center of gravity imbalance distance value and center of gravity imbalance angle value are comprehensively analyzed to obtain the posture abnormality coefficient.

3. A robot protection control method based on motion state detection according to claim 2, characterized in that: The specific method for obtaining the center of gravity deviation distance is as follows: Set the robot center position coordinates to , the robot's center of gravity coordinates are ; ,in Indicates the distance of center of gravity deviation. The x-axis coordinate of the robot's center of gravity. The y-axis coordinate of the robot's center of gravity. The z-axis coordinate of the robot's center of gravity.

4. A robot protection control method based on motion state detection according to claim 2, characterized in that: The specific method for obtaining the center of gravity deflection angle is as follows: Set three unit vectors to get the unit roll angle vector of the x-axis, the unit pitch angle vector of the y-axis, and the unit yaw angle vector of the z-axis; Set the center of gravity deviation vector; Calculate the dot product of the center of gravity deviation vector and the unit roll angle vector of the x-axis to get the X angle; Calculate the dot product of the center of gravity deflection vector and the unit pitch angle vector of the y-axis to get the Y angle; Calculate the dot product of the center of gravity deflection vector and the unit yaw angle vector of the z-axis to get the Z angle; The X angle, Y angle and Z angle are combined to obtain the center of gravity deflection angle.

5. The robot protection control method based on motion state detection according to claim 1 is characterized in that: In step 2, the robot imbalance threshold is set and compared with the robot imbalance assessment value; If the robot imbalance assessment value is greater than or equal to the robot imbalance threshold, the robot is judged to be unbalanced; If the robot imbalance assessment value is less than the robot imbalance threshold, the robot is judged to be balanced.

6. The robot protection control method based on motion state detection according to claim 1 is characterized in that: The specific method of obtaining the safety anomaly coefficient is as follows: The vibration intensities are summed and calculated to obtain the sum of the vibration intensities; The sum of the vibration intensities is averaged to obtain a first average vibration intensity; Calculating and summing a plurality of first average vibration intensities to obtain a sum of the vibration intensities within a monitoring time period; The sum of the vibration intensities within the monitoring time period is averaged to obtain a second average vibration intensity; Performing variance calculation on a number of first average vibration intensities and second average vibration intensities to obtain a vibration intensity fluctuation value; The vibration frequencies are summed up to obtain the sum of the vibration frequencies; The sum of the vibration frequencies is averaged to obtain the average vibration frequency; The variance of several vibration frequencies and the average vibration frequency is calculated to obtain the vibration frequency fluctuation value; A comprehensive analysis is conducted on the vibration intensity fluctuation value and the vibration frequency fluctuation value to obtain the safety abnormality coefficient.

7. The robot protection control method based on motion state detection according to claim 1 is characterized in that: In step 5, the robot avoidance threshold is set and compared with the robot avoidance evaluation value; If the robot avoidance evaluation value is greater than or equal to the robot avoidance threshold, it is determined that the robot needs to avoid; If the robot avoidance evaluation value is less than the robot avoidance threshold, it is determined that the robot does not need to avoid.

8. A robot protection control device based on motion state detection, used in a robot protection control method based on motion state detection according to any one of claims 1 to 7, characterized in that: include: Data acquisition module, data analysis module, data execution module, joint motor module and positioning sensor module; The data acquisition module is used to acquire motion state data, external collision data and robot vibration data and pre-process them, and send the pre-processed data to the data analysis module; The data analysis module is used to receive the data sent by the data acquisition module, analyze and judge the pre-processed motion state data, external collision data and robot vibration data, and send the results of the analysis and judgment to the data execution module; The data execution module is used to receive the results of analysis and judgment by the data analysis module, and adjust the results of imbalance judgment and avoidance judgment of the robot, and send the adjustment instructions to the joint motor module and the positioning sensor module; The joint motor module is used to receive the adjustment instruction of the data execution module, adjust the joint angle of the robot and lower the center of gravity of the robot; The positioning sensor module is used to receive the adjustment instruction of the data execution module and perform avoidance according to the acquired real-time position data and the path avoidance algorithm.

Images