A method for stabilizing the posture of a robotic arm based on vibration feedback

Abstract

The present invention relates to the technical field of agricultural robot motion control, and specifically to a method for stabilizing the posture of a robotic arm based on vibration feedback, comprising the following steps: S1: real-time acquisition of vibration signals from the end effector of the robotic arm; S2: frequency domain analysis and amplitude-phase decomposition of the vibration signals to extract vibration characteristic parameters of the robotic arm in the X / Y / Z axis directions; S3: generating a multi-axis vibration vector in combination with terrain inclination angle data; S4: calculating the posture compensation parameters of each joint; S5: generating a robotic arm joint angle adjustment instruction based on the posture compensation parameters; S6: looping through S1 to S5 until the vibration amplitude is lower than a preset threshold. The present invention achieves rapid and stable posture control of the robotic arm in a high-frequency disturbance environment by extracting multi-axis vibration characteristics in real time and calculating the joint compensation angle using anti-drift and anti-overturning matrices, thereby significantly improving the end positioning accuracy and overall operational reliability.

Description

Technical Field

[0001] The present invention relates to the technical field of agricultural robot motion control, and in particular to a method for stabilizing the posture of a robotic arm based on vibration feedback. Background Art

[0002] In the fields of industrial automation and precision manipulation, robotic arms have been widely used in tasks such as assembly, handling, welding, and medical assistance. However, during actual operation, the end effector of the robotic arm is often affected by the vibration of the work platform, uneven terrain, sudden motion changes, or external disturbance forces, resulting in small vibrations in multiple axes and even posture deviations, which in turn affects its end positioning accuracy and trajectory repeatability.

[0003] Traditional robotic arm attitude control methods often rely on closed-loop correction mechanisms based on position errors. These methods often only respond to static deviations or low-frequency drifts, but struggle to promptly address the cumulative errors caused by high-frequency, small-amplitude, but continuous disturbances. Furthermore, current methods generally lack the ability to identify and model the structural characteristics of the vibration source, effectively separating the coupling relationship between translational vibration and rotational disturbances. This results in a delayed control strategy response to disturbances and insufficient adjustment accuracy. Therefore, a method for attitude stabilization based on high-frequency dynamic characteristics is urgently needed to address these issues. Summary of the Invention

[0004] Based on the above objectives, the present invention provides a method for stabilizing the posture of a robotic arm based on vibration feedback.

[0005] A method for adjusting the posture stability of a robotic arm based on vibration feedback comprises the following steps:

[0006] S1: The vibration sensor group installed at the end of the robot arm collects the vibration signal of the end effector of the robot arm in real time;

[0007] S2: Perform frequency domain analysis and amplitude-phase decomposition on the vibration signal collected by S1 to extract the vibration characteristic parameters of the robot arm in the X / Y / Z axis directions. The vibration characteristic parameters include the main vibration frequency, amplitude distribution and phase offset;

[0008] S3: Inputting the vibration characteristic parameters into the terrain detection module and combining them with the terrain inclination angle data to generate a multi-axis vibration vector, wherein the multi-axis vibration vector includes a translation vibration component and a rotation vibration component;

[0009] S4: Construct anti-drift coefficient matrix and anti-rollover coefficient matrix based on multi-axis vibration vector, and calculate the posture compensation parameters of each joint through the back-drive algorithm;

[0010] S5: Generate a robot arm joint angle adjustment instruction based on the posture compensation parameters to drive each joint of the robot arm to perform a reverse compensation action;

[0011] S6: Steps S1 to S5 are executed repeatedly until the vibration amplitude of the end effector of the robot arm is lower than a preset threshold.

[0012] Optionally, the S1 specifically includes:

[0013] S11: Install at least three triaxial MEMS acceleration sensors along the three orthogonal directions of X, Y, and Z on the fixed bracket of the end effector of the robotic arm, so as to synchronously detect the acceleration change value of the end effector in three-dimensional space;

[0014] S12: Connect the output signals of each triaxial acceleration sensor to a high-speed data acquisition device with a sampling frequency of 1000 Hz and a data accuracy of 16 bits, and synchronize time-stamp the multi-channel vibration data based on a unified clock signal;

[0015] S13: performing digital band-pass filtering on the collected original vibration signal, wherein the band-pass frequency range of the band-pass filter is set to 10 Hz to 500 Hz, so as to reduce low-frequency drift and high-frequency interference.

[0016] Optionally, the S2 specifically includes:

[0017] S21: The acceleration signals collected in S1 are grouped according to the three axes of X, Y, and Z, and each group of acceleration time series is normalized to zero mean;

[0018] S22: Perform frequency domain transformation on each set of acceleration time series using fast Fourier transform to obtain a spectrum density distribution diagram of the corresponding axis, and identify the frequency component with the largest amplitude in the spectrum, and use this frequency as the main vibration frequency of the axis;

[0019] S23: Based on the frequency domain transformation result, the amplitude distribution corresponding to each frequency component in the spectrum along each axis is counted, and the relationship between the amplitude peak and its frequency is recorded as the amplitude distribution feature of the corresponding axis;

[0020] S24: By comparing the phase differences of different axial frequency components, the phase offset between the main frequency components is extracted.

[0021] Optionally, the S3 specifically includes:

[0022] S31: Using the three axial vibration characteristic parameters extracted in S2 as input vector elements to construct an initial vibration state vector, wherein the three vibration characteristic parameters include the vibration main frequency, amplitude distribution and phase offset in the X-axis, Y-axis and Z-axis directions;

[0023] S32: Acquire terrain inclination angle data of the current robotic arm working area, wherein the terrain inclination angle data is measured by a preset inclination sensor and includes two data items: a pitch angle and a roll angle relative to a horizontal plane;

[0024] S33: performing a coordinate rotation transformation on the initial vibration state vector based on the terrain inclination angle data, mapping the vibration parameters in the local coordinate system to the global terrain coordinate system, thereby obtaining a vibration state description after terrain correction;

[0025] S34: Based on the acceleration projection values ​​in each axis after transformation, respectively calculate the synthetic vibration component in the linear translation direction and the rotational vibration component caused by the center of mass offset, and combine them to construct a multi-axis vibration vector.

[0026] Optionally, the S34 specifically includes:

[0027] S341: Perform projection calculation on the vibration state vector after terrain correction to obtain the components of the X-axis, Y-axis and Z-axis in the terrain coordinate system, which are recorded as ;

[0028] S342: Substitute the acceleration component into the translation vector calculation expression to obtain the synthetic vibration component in the linear translation direction , the specific formula is; ;

[0029] S343: Obtain the structural geometric parameters of the end effector of the robotic arm, including the center of mass coordinates , the distance relative to the center of rotation of each axis;

[0030] S344: Calculate the rotational inertia response induced by translational excitation based on the acceleration components and the center of mass offset distance, obtain the rotational vibration components in three directions, and then calculate the composite value of the rotation vector , and its calculation formula is: ,in; They represent the offset distances of the center of mass of the end effector relative to the X, Y, and Z axes, respectively.

[0031] Optionally, the S4 specifically includes:

[0032] S41: Split the multi-axis vibration vector obtained in S3 into a linear translation vibration component and a rotation vibration component, which are recorded as a translation vector and a rotation vector respectively;

[0033] S42: Based on the distribution ratio of the translation vector in each direction and the length of the connecting arm between the end effector and each joint, the offset response strength of the robot arm to each joint in different postures is calculated, and then a three-dimensional anti-offset coefficient matrix is ​​constructed;

[0034] S43: Based on the distribution direction of the rotation vector and the relative position of the center of mass of each segment of the manipulator, the transmission relationship of the vibration energy to each rotational degree of freedom is evaluated, and the anti-overturning coefficient matrix is ​​constructed in combination with the moment of inertia distribution;

[0035] S44: Using the above two coefficient matrices as input parameters, combined with the initial angles, joint stiffnesses and end disturbance directions of each joint of the current manipulator, an inverse posture calculation is performed through a reverse drive algorithm to obtain the compensation angle correction value of each joint in the current posture;

[0036] S45: Integrate the correction amount into a posture compensation parameter set.

[0037] Optionally, the S42 specifically includes:

[0038] S421: Read the acceleration amplitude of the linear translation vibration component in the X, Y, and Z axes, which are recorded as ; And normalize the three values ​​to get the direction weight ;

[0039] S422: Measure the length of the connecting arm between the end effector and the nearest joint of the robotic arm in the X, Y, and Z directions, respectively. At the same time, the linear stiffness coefficients in three directions are obtained based on the material test results and are recorded as ;

[0040] S423: Calculate the anti-drift coefficient for each axis by combining the directional weight, connecting arm length and stiffness coefficient , the specific formula is: ;

[0041] S424: Arrange them in sequence on the main diagonal position of the third-order square matrix to construct a three-dimensional anti-skew coefficient matrix R.

[0042] Optionally, the S43 specifically includes:

[0043] S431: Extract the angular acceleration values ​​of the rotational vibration component in the X, Y, and Z axes, respectively, , and based on the square sum normalization process, the angular acceleration weight coefficients in each direction are obtained ;

[0044] S432: Get the moment of inertia of the robot arm in the X, Y, and Z directions, which are recorded as ;

[0045] S433: According to the center of mass distribution, determine the anti-overturning lever arm length of each joint segment, which is recorded as , represents the distance between the torque action point and the rotation axis in the corresponding rotation direction;

[0046] S434: Calculate the anti-overturning coefficient in the X, Y, and Z axes based on the direction weight, moment of inertia, and lever arm length , the specific formula is: ;

[0047] S435: Assemble the anti-overturning coefficients in three directions into main diagonal elements to construct the anti-overturning coefficient matrix T.

[0048] Optionally, the S44 specifically includes:

[0049] S441: Read the three-dimensional anti-drift coefficient matrix R and anti-overturning coefficient matrix T constructed in S42 and S43 respectively, as well as the initial angle q, joint stiffness k and end disturbance direction vector of each joint ;

[0050] S442: Decompose the end disturbance vector v in three-dimensional space and apply it to each joint through matrix transformation to obtain the equivalent external force on each joint and equivalent external torque ;

[0051] S443: For each joint j, based on the equivalent external force , equivalent external torque And the stiffness k of the joint, calculate the theoretical compensation angle correction, the calculation formula is: ,in, represents the compensation angle correction of the j-th joint;

[0052] S444: Integrate the compensation angle correction values ​​of all joints into a compensation parameter set .

[0053] Optionally, the S5 specifically includes:

[0054] S51: The posture compensation parameter set output in step S44 is Input the motion control instruction building unit to obtain the target adjustment angle of each joint, where represents the angle correction of the j-th joint;

[0055] S52: Read the current actual angle status of each joint , and the corresponding correction amount Add together to get the new target angle value , the specific calculation is as follows: ;

[0056] S53: Target Angle Perform amplitude limiting to ensure that it is within the preset range of the physical movement of the joint. Specifically, set the minimum movement angle of the j-th joint to , the maximum action angle is , then the final control angle The calculation formula is: ;

[0057] S54: Set the command angles of all joints It is encapsulated as a robotic arm control instruction set and sent to the servo actuator according to the control cycle to drive each joint to perform corresponding angle movements.

[0058] Beneficial effects of the present invention:

[0059] By deploying multiple sets of three-axis vibration sensors at the end of the robotic arm and combining frequency domain analysis and amplitude-phase decomposition technology, the present invention can accurately extract the main vibration frequency, amplitude distribution and phase offset in the X, Y and Z directions. Furthermore, combined with terrain inclination angle information, a multi-axis vibration vector coupled with translation and rotation is constructed to effectively identify the type and direction of disturbance, providing a data basis for subsequent precise control. Compared with traditional position error feedback methods, this method has a higher dynamic response rate and vibration identification accuracy.

[0060] The present invention converts the end disturbance into force / torque input of each joint by constructing an anti-drift coefficient matrix and an anti-overturning coefficient matrix, calculates the compensation angle correction using a back-drive algorithm, and generates posture adjustment instructions that comply with physical constraints, thereby realizing real-time stable control of a multi-degree-of-freedom manipulator in a high-frequency disturbance environment. This method not only improves the robustness and coordination of posture control, but also significantly improves the operating accuracy and reliability of the manipulator end under complex task conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only for the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0062] Figure 1 Schematic diagram of a method for adjusting the posture stability of a robotic arm according to an embodiment of the present invention;

[0063] Figure 2 The figure is a schematic diagram of the process of generating posture compensation parameters according to an embodiment of the present invention. DETAILED DESCRIPTION

[0064] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments. It is also noted that, to provide a more detailed description, the following embodiments are best and preferred embodiments, and those skilled in the art may employ alternative methods for implementing certain known technologies. Furthermore, the accompanying drawings are intended only to provide a more detailed description of the embodiments and are not intended to limit the present invention.

[0065] like Figure 1-Figure 2 As shown, a method for stabilizing the posture of a robotic arm based on vibration feedback includes the following steps:

[0066] S1: The vibration sensor group installed at the end of the robot arm collects the vibration signal of the end effector of the robot arm in real time;

[0067] S2: Perform frequency domain analysis and amplitude-phase decomposition on the vibration signal collected by S1 to extract the vibration characteristic parameters of the robot arm in the X / Y / Z axis directions. The vibration characteristic parameters include the main vibration frequency, amplitude distribution and phase offset;

[0068] S3: Input the vibration characteristic parameters into the terrain detection module and generate a multi-axis vibration vector in combination with the terrain inclination angle data. The multi-axis vibration vector includes a translation vibration component and a rotation vibration component.

[0069] S4: Construct anti-drift coefficient matrix and anti-rollover coefficient matrix based on multi-axis vibration vector, and calculate the posture compensation parameters of each joint through the back-drive algorithm;

[0070] S5: Generate a robot arm joint angle adjustment instruction based on the posture compensation parameters to drive each joint of the robot arm to perform a reverse compensation action;

[0071] S6: Steps S1 to S5 are executed repeatedly until the vibration amplitude of the end effector of the robot arm is lower than a preset threshold.

[0072] S1 specifically includes:

[0073] S11: Install at least three triaxial MEMS acceleration sensors along the three orthogonal directions of X, Y, and Z on the fixed bracket of the end effector of the robotic arm, so as to synchronously detect the acceleration change value of the end effector in three-dimensional space;

[0074] S12: Connect the output signals of each triaxial acceleration sensor to a high-speed data acquisition device with a sampling frequency of 1000 Hz and a data accuracy of 16 bits, and synchronize time-stamp the multi-channel vibration data based on a unified clock signal;

[0075] S13: Perform digital bandpass filtering on the collected raw vibration signal. The bandpass frequency range of the bandpass filter is set to 10Hz to 500Hz to reduce low-frequency drift and high-frequency interference. The above steps can obtain low-noise, high-fidelity vibration signals within millisecond time resolution through multiple groups of orthogonally installed three-axis acceleration sensors combined with a high-sampling rate data acquisition card and fixed-passband digital filtering. This provides a reliable data basis for subsequent vibration feature analysis and posture compensation calculations.

[0076] S2 specifically includes:

[0077] S21: The acceleration signals collected in S1 are grouped according to the three axes of X, Y, and Z, and each group of acceleration time series is normalized to zero mean to eliminate the interference of DC offset on subsequent frequency domain analysis;

[0078] S22: Perform frequency domain transformation on each set of acceleration time series using fast Fourier transform to obtain a spectrum density distribution diagram of the corresponding axis, and identify the frequency component with the largest amplitude in the spectrum, and use this frequency as the main vibration frequency of the axis;

[0079] S22 specifically includes:

[0080] S221: Extract each set of acceleration time series collected in S1 along the axial direction, and intercept a data window of length N from each series for frequency domain analysis, where N represents the number of sampling points of the acceleration series;

[0081] S222: Perform a fast Fourier transform operation on each acceleration time series of length N to obtain its complex frequency response in the frequency domain , the transformation process is calculated using the following formula:

[0082] ,in, represents the complex frequency domain response of the kth frequency component; Indicates the acceleration value of the nth sampling point in the time series; i represents the imaginary unit; k represents the frequency index, and its value range is ;

[0083] S223: Calculate the amplitude modulus of each frequency component based on the fast Fourier transform results , the amplitude modulus is used to represent the vibration energy distribution at the corresponding frequency, and the specific calculation method is:

[0084] ,in, represents the amplitude modulus of the kth frequency component; and They are The real and imaginary parts of

[0085] S224: Traverse the amplitude modulus of all frequency components and select the frequency corresponding to the frequency component with the largest amplitude As the main frequency of the corresponding axial vibration signal, it is calculated as follows: ,in, Indicates the main frequency of vibration; Indicates The maximum frequency index value; F represents the acceleration sampling frequency; N is the same as above, representing the number of sampling points.

[0086] S23: Based on the frequency domain transformation result, the amplitude distribution corresponding to each frequency component in the spectrum along each axis is counted, and the relationship between the amplitude peak and its frequency is recorded as the amplitude distribution feature of the corresponding axis;

[0087] S24: By comparing the phase differences of different axial frequency components, the phase offset between the main frequency components is extracted. The phase offset reflects the synchronization and coupling relationship of the robot arm's vibration in different directions.

[0088] S3 specifically includes:

[0089] S31: Using the three axial vibration characteristic parameters extracted in S2 as input vector elements, the initial vibration state vector is constructed. The three vibration characteristic parameters include the vibration main frequency, amplitude distribution and phase offset in the X-axis, Y-axis and Z-axis directions;

[0090] S32: Acquire terrain inclination angle data of the current robotic arm working area. The terrain inclination angle data is measured by a preset inclination sensor and includes two data items: a pitch angle and a roll angle relative to the horizontal plane.

[0091] S33: performing a coordinate rotation transformation on the initial vibration state vector based on the terrain inclination angle data, mapping the vibration parameters in the local coordinate system to the global terrain coordinate system, thereby obtaining a vibration state description after terrain correction;

[0092] S34: Based on the acceleration projection values ​​in each axis after transformation, the synthetic vibration component in the linear translation direction and the rotational vibration component caused by the center of mass offset are calculated respectively, and they are combined to construct a multi-axis vibration vector; the above steps can accurately identify the multi-axis dynamic characteristics of the robot arm under complex tilt conditions by correcting the vibration characteristic parameters in the terrain coordinate system and distinguishing between the translation and rotation response forms, providing a more spatially matched posture data basis for subsequent compensation control.

[0093] S34 specifically includes:

[0094] S341: Perform projection calculation on the vibration state vector after terrain correction to obtain the components of the X-axis, Y-axis and Z-axis in the terrain coordinate system, which are recorded as , used to reflect the instantaneous acceleration value along the three-axis linear translation direction;

[0095] S342: Substitute the acceleration component into the translation vector calculation expression to obtain the synthetic vibration component in the linear translation direction , this synthetic component represents the overall center of mass vibration amplitude caused by multi-axis disturbance at the end of the manipulator. The specific formula is: ;

[0096] S343: Obtain the structural geometric parameters of the end effector of the robotic arm, including the center of mass coordinates , the distance relative to the center of rotation of each axis;

[0097] S344: Calculate the rotational inertia response induced by translational excitation based on the acceleration components and the center of mass offset distance, obtain the rotational vibration components in three directions, and then calculate the composite value of the rotation vector , and its calculation formula is: ,in; Respectively represent the offset distance of the end effector's center of mass relative to the X, Y, and Z axes; The meaning is the same as above; by simultaneously calculating the rotational effect caused by the end acceleration projection and the center of mass offset, the linear and rotational vibration components can be clearly distinguished, which can accurately characterize the dynamic response characteristics of the robot arm in complex posture and terrain environments, and provide a directional separation compensation basis for the anti-vibration control strategy.

[0098] S4 specifically includes:

[0099] S41: Split the multi-axis vibration vector obtained in S3 into a linear translation vibration component and a rotation vibration component, which are respectively recorded as a translation vector and a rotation vector, and construct a three-dimensional vibration influence ratio according to the numerical values ​​of each component in the X, Y, and Z directions;

[0100] S42: Based on the distribution ratio of the translation vector in each direction and the length of the connecting arm between the end effector and each joint, the offset response strength of the robot arm to each joint in different postures is calculated, and then a three-dimensional anti-offset coefficient matrix is ​​constructed to quantify the linear vibration suppression ability of different joints;

[0101] S43: Based on the distribution direction of the rotation vector and the relative position of the center of mass of each segment of the manipulator, the transmission relationship of vibration energy to each rotational degree of freedom is evaluated. In combination with the moment of inertia distribution, an anti-overturning coefficient matrix is ​​constructed to characterize the stable load-bearing capacity of each joint against rotational vibration disturbances.

[0102] S44: Using the above two coefficient matrices as input parameters, combined with the initial angles, joint stiffnesses and end disturbance directions of each joint of the current manipulator, an inverse posture calculation is performed through a reverse drive algorithm to obtain the compensation angle correction value of each joint in the current posture;

[0103] S45: Integrate the correction amount into a posture compensation parameter set as the input basis for subsequent instruction generation; the above steps construct anti-offset and anti-overturning matrices related to the energy response, and combine the reverse drive algorithm to perform posture inverse solution, which can accurately match the actual vibration direction with the structural characteristics of each joint of the robotic arm, and realize multi-joint collaborative compensation control under complex three-dimensional disturbance conditions.

[0104] S42 specifically includes:

[0105] S421: Read the acceleration amplitude of the linear translation vibration component in the X, Y, and Z axes, which are recorded as ; And normalize the three values ​​to get the direction weight The specific formula is as follows: ; ; ,in, represents the acceleration of each axis, represents the normalized direction weight;

[0106] S422: Measure the length of the connecting arm between the end effector and the nearest joint of the robotic arm in the X, Y, and Z directions, respectively. At the same time, the linear stiffness coefficients in three directions are obtained based on the material test results and are recorded as ;

[0107] S423: Calculate the anti-drift coefficient for each axis by combining the directional weight, connecting arm length and stiffness coefficient , the specific formula is: ;

[0108] S424: Arrange them in sequence on the main diagonal position of the third-order square matrix to construct a three-dimensional anti-skew coefficient matrix R, which is expressed as: The above steps quantify the linear acceleration amplitude, connecting arm length and stiffness coefficient into an anti-drift coefficient matrix, which can accurately reflect the rigidity of the robot arm to resist linear vibration disturbances in three-dimensional space, and provide quantitative weights for subsequent reverse drive calculations, thereby improving the convergence speed and stability accuracy of posture compensation.

[0109] S43 specifically includes:

[0110] S431: Extract the angular acceleration values ​​of the rotational vibration component in the X, Y, and Z axes, respectively, , and based on the square sum normalization process, the angular acceleration weight coefficients in each direction are obtained The specific formula is as follows: ; ; ;in, represents the angular acceleration of each axis, represents the normalized weight;

[0111] S432: Get the moment of inertia of the robot arm in the X, Y, and Z directions, which are recorded as ,The moment of inertia is obtained by calculating the manipulator structure and the mass distribution of each joint;

[0112] S433: According to the center of mass distribution, determine the anti-overturning lever arm length of each joint segment, which is recorded as , represents the distance between the torque action point and the rotation axis in the corresponding rotation direction;

[0113] S434: Calculate the anti-overturning coefficient in the X, Y, and Z axes based on the direction weight, moment of inertia, and lever arm length , the specific formula is: ,in, Respectively represent the anti-overturning capacity on the X, Y, and Z axes;

[0114] S435: Assemble the anti-overturning coefficients in the three directions into main diagonal elements to construct the anti-overturning coefficient matrix T, which is expressed as: The above steps introduce the moment of inertia, center of mass structure and rotational disturbance strength of each axis of the robotic arm to construct a three-dimensional anti-overturning coefficient matrix, which can comprehensively characterize the anti-overturning ability of the system in different rotation directions and provide quantitative support for the implementation of the reverse compensation algorithm based on dynamic posture disturbance.

[0115] S44 specifically includes:

[0116] S441: Read the three-dimensional anti-drift coefficient matrix R and anti-overturning coefficient matrix T constructed in S42 and S43 respectively, as well as the initial angle q, joint stiffness k and end disturbance direction vector of each joint ;

[0117] S442: Decompose the end disturbance vector v in three-dimensional space and apply it to each joint through matrix transformation to obtain the equivalent external force on each joint and equivalent external torque , the formula is: ; ,in, represents the equivalent external force of the j-th joint; represents the equivalent external torque of the j-th joint;

[0118] S443: For each joint j, based on the equivalent external force , equivalent external torque And the stiffness k of the joint, calculate the theoretical compensation angle correction, the calculation formula is: ,in, represents the compensation angle correction of the j-th joint;

[0119] S444: Integrate the compensation angle correction values ​​of all joints into a compensation parameter set , to guide subsequent posture compensation actions; through the above steps, efficient inverse compensation control of multi-joint postures can be achieved, and the posture maintenance ability and dynamic response accuracy of the robot arm in a multi-dimensional disturbance environment can be improved.

[0120] S5 specifically includes:

[0121] S51: The posture compensation parameter set output in step S44 is Input the motion control instruction building unit to obtain the target adjustment angle of each joint, where represents the angle correction of the j-th joint;

[0122] S52: Read the current actual angle status of each joint , and the corresponding correction amount Add together to get the new target angle value , the specific calculation is as follows: ;

[0123] S53: Target Angle Perform amplitude limiting to ensure that it is within the preset range of the physical movement of the joint. Specifically, set the minimum movement angle of the j-th joint to , the maximum action angle is , then the final control angle The calculation formula is: ;

[0124] S54: Set the command angles of all joints The instructions are encapsulated as a set of robot arm control instructions and sent to the servo actuator according to the control cycle to drive each joint to perform the corresponding angle movement and realize posture adjustment. The above steps can realize the accurate generation of joint angle control instructions through the joint calculation of posture compensation parameters and current state, motion range verification and instruction encapsulation, ensuring that the compensation instructions are executed efficiently and safely within the range allowed by the physical structure, thereby realizing real-time and stable adjustment of the collaborative posture of multiple joints of the robot arm.

[0125] The present invention encompasses any alternatives, modifications, equivalents, and solutions that fall within the spirit and scope of the present invention. To provide a thorough understanding of the present invention, specific details are described in detail below in connection with the preferred embodiments of the present invention, but those skilled in the art will be able to fully understand the present invention without these detailed descriptions. Furthermore, to avoid unnecessary confusion regarding the essence of the present invention, well-known methods, processes, procedures, components, and circuits have not been described in detail.

[0126] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.

Claims

1. A method for adjusting the posture stability of a robotic arm based on vibration feedback, characterized in that: The following steps are involved: S1: The vibration sensor group installed at the end of the robot arm collects the vibration signal of the end effector of the robot arm in real time; S2: Perform frequency domain analysis and amplitude-phase decomposition on the vibration signal collected by S1 to extract the vibration characteristic parameters of the robot arm in the X / Y / Z axis directions. The vibration characteristic parameters include the main vibration frequency, amplitude distribution and phase offset; S3: Inputting the vibration characteristic parameters into the terrain detection module and combining them with the terrain inclination angle data to generate a multi-axis vibration vector, wherein the multi-axis vibration vector includes a translation vibration component and a rotation vibration component; The S3 specifically includes: S31: Using the three axial vibration characteristic parameters extracted in S2 as input vector elements to construct an initial vibration state vector, wherein the three vibration characteristic parameters include the vibration main frequency, amplitude distribution and phase offset in the X-axis, Y-axis and Z-axis directions; S32: Acquire terrain inclination angle data of the current robotic arm working area, wherein the terrain inclination angle data is measured by a preset inclination sensor and includes two data items: a pitch angle and a roll angle relative to a horizontal plane; S33: performing a coordinate rotation transformation on the initial vibration state vector based on the terrain inclination angle data, mapping the vibration parameters in the local coordinate system to the global terrain coordinate system, thereby obtaining a vibration state description after terrain correction; S34: Calculate the synthetic vibration component in the linear translation direction and the rotational vibration component caused by the center of mass shift according to the acceleration projection values ​​in each axis after the transformation, and combine them to construct a multi-axis vibration vector; The S34 specifically includes: S341: Perform projection calculation on the vibration state vector after terrain correction to obtain the components of the X-axis, Y-axis and Z-axis in the terrain coordinate system, which are recorded as ; S342: Substitute the acceleration component into the translation vector calculation expression to obtain the synthetic vibration component in the linear translation direction , the specific formula is: ; S343: Obtain the structural geometric parameters of the end effector of the robotic arm, including the center of mass coordinates , the distance relative to the center of rotation of each axis; S344: Calculate the rotational inertia response induced by translational excitation based on the acceleration components and the center of mass offset distance, obtain the rotational vibration components in three directions, and then calculate the composite value of the rotation vector , and its calculation formula is: ,in; Respectively represent the offset distance of the end effector's center of mass relative to the X, Y, and Z axes; S4: Construct anti-drift coefficient matrix and anti-rollover coefficient matrix based on multi-axis vibration vector, and calculate the posture compensation parameters of each joint through the back-drive algorithm; The S4 specifically includes: S41: Split the multi-axis vibration vector obtained in S3 into a linear translation vibration component and a rotation vibration component, which are recorded as a translation vector and a rotation vector respectively; S42: Based on the distribution ratio of the translation vector in each direction and the length of the connecting arm between the end effector and each joint, the offset response strength of the robot arm to each joint in different postures is calculated, and then a three-dimensional anti-offset coefficient matrix is ​​constructed; S43: Based on the distribution direction of the rotation vector and the relative position of the center of mass of each segment of the manipulator, the transmission relationship of the vibration energy to each rotational degree of freedom is evaluated, and the anti-overturning coefficient matrix is ​​constructed in combination with the moment of inertia distribution; S44: Using the above two coefficient matrices as input parameters, combined with the initial angles, joint stiffnesses and end disturbance directions of each joint of the current manipulator, an inverse posture calculation is performed through a reverse drive algorithm to obtain the compensation angle correction value of each joint in the current posture; S45: Integrate the correction amount into an attitude compensation parameter set; S5: Generate a robot arm joint angle adjustment instruction based on the posture compensation parameters to drive each joint of the robot arm to perform a reverse compensation action; S6: Steps S1 to S5 are executed repeatedly until the vibration amplitude of the end effector of the robot arm is lower than a preset threshold.

2. The method for adjusting the posture stability of a robotic arm based on vibration feedback according to claim 1, characterized in that: Said S1 specifically includes: S11: Install at least three triaxial MEMS acceleration sensors along the three orthogonal directions of X, Y, and Z on the fixed bracket of the end effector of the robotic arm, so as to synchronously detect the acceleration change value of the end effector in three-dimensional space; S12: Connect the output signals of each triaxial acceleration sensor to a high-speed data acquisition device with a sampling frequency of 1000 Hz and a data accuracy of 16 bits, and synchronize time-stamp the multi-channel vibration data based on a unified clock signal; S13: Perform digital band-pass filtering on the collected original vibration signal. The band-pass frequency range of the band-pass filter is set to 10 Hz to 500 Hz to reduce low-frequency drift and high-frequency interference.

3. The method for adjusting the posture stability of a robotic arm based on vibration feedback according to claim 1, characterized in that: The S2 specifically includes: S21: The acceleration signals collected in S1 are grouped according to the three axes of X, Y, and Z, and each group of acceleration time series is normalized to zero mean; S22: Perform frequency domain transformation on each set of acceleration time series using fast Fourier transform to obtain a spectrum density distribution diagram of the corresponding axis, and identify the frequency component with the largest amplitude in the spectrum, and use this frequency as the main vibration frequency of the axis; S23: Based on the frequency domain transformation result, the amplitude distribution corresponding to each frequency component in the spectrum along each axis is counted, and the relationship between the amplitude peak and its frequency is recorded as the amplitude distribution feature of the corresponding axis; S24: By comparing the phase differences of different axial frequency components, the phase offset between the main frequency components is extracted.

4. The method for adjusting the posture stability of a robotic arm based on vibration feedback according to claim 1, wherein: The S42 specifically includes: S421: Read the acceleration amplitude of the linear translation vibration component in the X, Y, and Z axes, which are recorded as ; And normalize the three values ​​to get the direction weights u, v, w; S422: Measure the length of the connecting arm between the end effector and the nearest joint of the robotic arm in the X, Y, and Z directions, which are recorded as l, m, and n respectively. At the same time, obtain the linear stiffness coefficients in the three directions based on the material test results, which are recorded as k, p, and q respectively. S423: Calculate the anti-drift coefficients r, s, and t for each axis by combining the directional weight, connecting arm length, and stiffness coefficient. The specific formula is: ; S424: Arrange r, s, and t in sequence on the main diagonal position of the third-order square matrix to construct a three-dimensional anti-skew coefficient matrix R.

5. The method for adjusting the posture stability of a robotic arm based on vibration feedback according to claim 4, characterized in that: The S43 specifically includes: S431: Extract the angular acceleration values ​​of the rotational vibration component in the X, Y, and Z axes, respectively, , and based on the square sum normalization process, the angular acceleration weight coefficients in each direction are obtained ; S432: Obtain the moment of inertia of the robot arm in the X, Y, and Z directions, which are recorded as I, J, and K respectively; S433: Determine the anti-overturning lever arm length of each joint segment based on the center of mass distribution, denoted as d, e, and f, respectively, representing the distance between the moment application point and the rotation axis in the corresponding rotation direction; S434: Calculate the anti-overturning coefficient in the X, Y, and Z axes based on the direction weight, moment of inertia, and lever arm length , the specific formula is: ; S435: Assemble the anti-overturning coefficients in three directions into main diagonal elements to construct the anti-overturning coefficient matrix T.

6. The method for adjusting the posture stability of a robotic arm based on vibration feedback according to claim 5, characterized in that: The S44 specifically includes: S441: Read the three-dimensional anti-drift coefficient matrix R and anti-overturning coefficient matrix T constructed in S42 and S43 respectively, as well as the initial angle q, joint stiffness k and end disturbance direction vector of each joint ; S442: Decompose the end disturbance vector v in three-dimensional space and apply it to each joint through matrix transformation to obtain the equivalent external force on each joint and equivalent external torque ; S443: For each joint j, based on the equivalent external force , equivalent external torque And the stiffness k of the joint, calculate the theoretical compensation angle correction, the calculation formula is: ,in, represents the compensation angle correction of the j-th joint; S444: Integrate the compensation angle correction values ​​of all joints into a compensation parameter set .

7. The method for adjusting the posture stability of a robotic arm based on vibration feedback according to claim 6, characterized in that: The S5 specifically includes: S51: The posture compensation parameter set output in step S44 is Input the motion control instruction building unit to obtain the target adjustment angle of each joint, where represents the angle correction of the j-th joint; S52: Read the current actual angle status of each joint , and the corresponding correction amount Add together to get the new target angle value , the specific calculation is as follows: ; S53: Target Angle Perform amplitude limiting to ensure that it is within the preset range of the physical movement of the joint. Specifically, set the minimum movement angle of the j-th joint to , the maximum action angle is , then the final control angle The calculation formula is: ; S54: Set the command angles of all joints It is encapsulated as a robotic arm control instruction set and sent to the servo actuator according to the control cycle to drive each joint to perform the corresponding angle movement.

Images