A control method, system and device for controlling a compliant robot joint gap

Abstract

The application discloses a control method, system and device for controlling robot joint clearance, which is used for eliminating impact sources from the root and improving the compliance control effect of the robot joint clearance. The method comprises the following steps: in each control cycle, real-time acquisition of motor side encoder data and connecting rod side encoder data of each joint of the robot; fusion of the motor side encoder data and the connecting rod side encoder data by using a state observer to obtain a relative state vector of the joint; acquisition of a relative position and a relative speed at the current moment from the relative state vector, and acquisition of a preset or online-identified total width of the joint clearance; calculation of a collision time required for the motor of the joint to reach the boundary of the joint clearance based on the relative position, the relative speed and the total width of the joint clearance; adjustment of impedance parameters in an impedance control law acting on the joint according to the collision time; calculation of a torque command based on the adjusted impedance parameters, and driving of the joint movement.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a control method, system and device for compliant control of the joint clearance of a robot. Background Technology

[0002] In the field of modern industrial automation, robots are widely used in high-precision tasks such as assembly, welding, and machining. The accuracy and smoothness of robot motion directly depend on the control performance of its joint servo system. A typical robot joint consists of components such as servo motors, reducers (such as harmonic reducers and RV reducers), and linkages. To ensure mechanical transmission, tiny gaps must be reserved between gears. This physical gap creates a nonlinear, discontinuous dead zone at the control level, which is one of the inherent and most significant performance limiting factors in robot servo control systems.

[0003] To overcome the adverse effects of joint clearance, existing technologies primarily employ compensation control strategies. Early approaches relied on offline calibrated clearance values, performing open-loop rapid position compensation—a process known as gap skipping—when the motor command reversed. More advanced methods introduce state observers, estimating the actual position of the link by establishing a nonlinear model of the clearance (such as a dead-zone model or a hysteresis model), and designing a closed-loop controller for error correction. However, the essence of existing technologies is to treat the clearance as a position error that must be eliminated as quickly as possible, allowing the motor to pass through this ineffective clearance region with maximum acceleration and speed. This approach leads to a rigid collision between the driven end and the driven end at a non-zero relative velocity after crossing the clearance, inevitably triggering shocks and vibrations. At this point, only passive suppression is possible after the impact occurs; the impact source cannot be fundamentally eliminated, thus affecting the compliant control effect of the joint clearance. Summary of the Invention

[0004] This application provides a control method, system, and apparatus for compliantly controlling the joint clearance of a robot, which eliminates the impact source at its source and improves the compliant control effect on the joint clearance of the robot.

[0005] The first aspect of this application provides a method for controlling the joint clearance of a compliant robot, including:

[0006] In each control cycle, the encoder data of the motor side and the encoder data of the link side of each joint of the robot are acquired in real time.

[0007] The relative state vector of the joint is obtained by fusing the encoder data of the motor side and the encoder data of the link side using a state observer. The relative state vector consists of relative position, relative velocity and relative acceleration.

[0008] obtaining a relative position and a relative velocity at a current time from the relative state vector, and obtaining a preset or online-identified total joint clearance width;

[0009] calculating a collision time required for a motor of the joint to reach a joint clearance boundary based on the relative position, the relative velocity, and the total joint clearance width;

[0010] adjusting an impedance parameter in an impedance control law acting on the joint according to the collision time;

[0011] calculating a torque command based on the adjusted impedance parameter, and driving the joint to move.

[0012] Optionally, the calculating the collision time required for the motor of the joint to reach the joint clearance boundary based on the relative velocity and the total joint clearance width comprises:

[0013] if a direction of the relative velocity is towards any boundary of the joint clearance, calculating a remaining distance between the relative position and the boundary according to the total joint clearance width;

[0014] dividing the remaining distance by an absolute value of the relative velocity to obtain the collision time.

[0015] Optionally, if the direction of the relative velocity is not towards any boundary of the joint clearance or the relative velocity is zero, setting the collision time as a preset saturation value.

[0016] Optionally, the impedance parameter in the impedance control law comprises a virtual damping parameter and a virtual stiffness parameter;

[0017] the virtual damping parameter is set as a first function, the first function being a continuous function with the collision time as an independent variable, and a function value of the first function being in an inverse relationship with the collision time;

[0018] the virtual stiffness parameter is set as a second function, the second function being a continuous function with a contact proximity as an independent variable, and a function value of the second function being in a proportional relationship with the contact proximity, the contact proximity being a scalar from 0 to 1 used to describe a spatial position of the motor in the joint clearance, and the contact proximity being calculated based on the relative position and the total joint clearance width.

[0019] Optionally, the adjusting the impedance parameter in the impedance control law acting on the joint according to the collision time comprises:

[0020] in each control cycle, inputting the calculated collision time and the contact proximity as inputs into the first function and the second function respectively to calculate a target virtual damping value and a target virtual stiffness value;

[0021] The torque instruction is calculated based on the adjusted impedance parameter, and the joint motion is driven, including:

[0022] The target virtual stiffness value, the target virtual damping value, a preset expected joint trajectory, and the continuous rod side encoder data are substituted into the impedance control law to calculate an impedance torque component;

[0023] A static feedforward torque component for compensating for gravity and friction is obtained, the impedance torque component and the static feedforward torque component are superimposed to obtain a final torque instruction, and the joint motion is driven.

[0024] Optionally, the method further includes:

[0025] When the collision time is less than a preset threshold, a transient feedforward torque component for actively canceling motor inertia is calculated based on the relative state vector and the collision time;

[0026] The static feedforward torque component for compensating for gravity and friction is obtained, the impedance torque component and the static feedforward torque component are superimposed to obtain a final torque instruction, and the joint motion is driven, including:

[0027] The impedance torque component, the static feedforward torque component, and the transient feedforward torque component are superimposed to obtain a final torque instruction, and the joint motion is driven.

[0028] Optionally, the fusion of the motor side encoder data and the continuous rod side encoder data by the state observer to obtain the relative state vector of the joint includes:

[0029] Based on a preset kinematic state transition model of the joint, a relative state vector output in a previous control period is recursively calculated to obtain a predicted vector in a current control period;

[0030] The motor side encoder data and the continuous rod side encoder data are fused into system measurement values, and a difference between the system measurement values and the predicted vector is calculated;

[0031] The predicted vector is corrected in combination with a Kalman gain calculated by the state observer and the difference to obtain an optimally estimated relative state vector in the current control period.

[0032] The second aspect of the present application provides a control system for controlling the joint clearance of a compliant control robot, including:

[0033] A first acquisition unit is configured to acquire motor side encoder data and continuous rod side encoder data of each joint of the robot in real time in each control period.

[0034] a fusion unit configured to fuse the motor-side encoder data and the link-side encoder data using a state observer to obtain a relative state vector of the joint, the relative state vector being composed of a relative position, a relative velocity and a relative acceleration;

[0035] a second obtaining unit configured to obtain the relative position and the relative velocity at a current time from the relative state vector, and obtain a total joint clearance width that is preset or identified online;

[0036] a first calculating unit configured to calculate a collision time required for a motor of the joint to reach a joint clearance boundary based on the relative position, the relative velocity and the total joint clearance width;

[0037] an adjusting unit configured to adjust an impedance parameter in an impedance control law acting on the joint according to the collision time;

[0038] a second calculating unit configured to calculate a torque command based on the adjusted impedance parameter, and drive the joint to move.

[0039] The third aspect of the present application provides a control device for controlling joint clearance of a compliant control robot, the control device comprising:

[0040] a processor, a memory, an input / output unit and a bus;

[0041] the processor is connected with the memory, the input / output unit and the bus;

[0042] the memory stores a program, and the processor invokes the program to execute the control method for controlling joint clearance of a compliant control robot according to the first aspect and any optional implementation of the first aspect.

[0043] The fourth aspect of the present application provides a computer readable storage medium, and the computer readable storage medium stores a program, and the program is executed on a computer to execute the control method for controlling joint clearance of a compliant control robot according to the first aspect and any optional implementation of the first aspect.

[0044] It can be seen from the above technical solutions that the present application has the following advantages:

[0045] The application does not directly correct the position, but uses the relative position and speed obtained by the state observer through real-time acquisition of double encoder data to calculate the collision time of the motor reaching the gap boundary in real time. The predicted collision time is used as the basis to actively adjust the impedance parameters in the impedance control law, thereby effectively reducing the relative speed of the motor and the connecting rod before contact occurs. This enables the control system to change its dynamic behavior in advance before the actual collision occurs, actively control the contact process to reduce the relative speed during contact, thereby avoiding rigid collision and eliminating the impact source from the root, achieving soft control of the robot joint gap. BRIEF DESCRIPTION OF DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the present application, the drawings needed in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0047] Figure 1 An embodiment flowchart of the control method for soft control of the robot joint gap provided by the present application is shown in the figure.

[0048] Figure 2 Another embodiment flowchart of the control method for soft control of the robot joint gap provided by the present application is shown in the figure.

[0049] Figure 3 An embodiment structure diagram of the control system for soft control of the robot joint gap provided by the present application is shown in the figure.

[0050] Figure 4 An embodiment structure diagram of the control device for soft control of the robot joint gap provided by the present application is shown in the figure. DETAILED DESCRIPTION

[0051] The present application provides a control method, system and device for soft control of the robot joint gap, which can eliminate the impact source from the root and improve the soft control effect of the robot joint gap.

[0052] Please refer to Figure 1 , Figure 1 An embodiment of the control method for soft control of the robot joint gap provided by the present application comprises the following steps.

[0053] 101. In each control cycle, real-time acquisition of motor side encoder data and connecting rod side encoder data of each joint of the robot is performed.

[0054] Traditional robot joints are usually equipped with only one motor-side encoder, but due to the existence of joint clearance, motor rotation does not equal to link rotation. When the motor idles within the joint clearance, the real state of the link cannot be known only by the motor encoder. Therefore, the embodiment needs to measure the motion state on both sides of the joint clearance at each control cycle, that is, to obtain the motor-side encoder data (driving end) and the link-side encoder data (driven end) of each joint of the robot. The motor-side encoder data reflects the input, and the link-side encoder data reflects the output, and the difference between the two sets of data directly corresponds to the real-time state of the joint in the clearance.

[0055] Among them, the motor-side encoder is installed on the output shaft of the servo motor, that is, the input end of the reducer / transmission mechanism. The link-side encoder is installed on the final output shaft of the joint, that is, the output end of the reducer / transmission mechanism, which is directly connected to the robot link. Since the physical size of the joint clearance is very small, the resolution of the encoder used must be high enough to ensure that the measurement accuracy is much higher than the width of the measured clearance.

[0056] 102, fuse the motor-side encoder data and the link-side encoder data by using a state observer to obtain the relative state vector of the joint, the relative state vector being composed of relative position, relative velocity and relative acceleration;

[0057] The encoder data can only directly reflect the position information, but to achieve predictive control, speed information and acceleration information must also be obtained. However, directly differentiating the position data to calculate the speed and acceleration will greatly amplify the noise, resulting in unusable results. Therefore, the motor-side encoder data and the link-side encoder data need to be fused by using a state observer to extract a smooth, complete and accurate relative state vector that can accurately reflect the real dynamics from the original incomplete and noisy measurement data.

[0058] The relative state vector is used to completely describe the relative dynamic relationship between the driving end and the driven end of the joint at a certain time, and the relative state vector is composed of three core elements: relative position Δθ, relative velocity Δθ˙ and relative acceleration Δθ¨, that is, the relative state vector x = [Δθ, Δθ˙, Δθ¨]T. Among them, Δθ is the relative position between the motor and the link, which directly reflects the position in the clearance; Δθ˙ is their relative velocity, which reflects how fast or slow it approaches or moves away from the contact clearance boundary; Δθ¨ is the relative acceleration, which indicates the trend of the dynamic force.

[0059] 103, obtain the relative position and relative velocity at the current time from the relative state vector, and obtain the total width of the joint clearance preset or identified online;

[0060] In order to calculate the collision time, in addition to obtaining the relative position and relative velocity at the current time from the relative state vector, another key physical parameter, i.e. the width of the joint clearance, needs to be obtained. There are two ways to obtain the total width of the joint clearance:

[0061] Method one: During system initialization or offline calibration, the joint clearance width is accurately measured by high-precision measurement tools (such as laser interferometer) or special calibration programs, and the value is stored as a fixed constant parameter in the controller configuration file. In each control cycle, this preset value can be directly read from the configuration. This method is simple to implement, but cannot adapt to changes in clearance due to wear or thermal expansion and contraction.

[0062] Method two: Through online identification, the estimate of the clearance width is updated autonomously during the robot operation. The principle of online identification is that when the joint motor instruction is reversed, the controller will monitor the entire process from "disengaging contact" to "contacting again". The relative position change obtained by step 102 can directly measure the total travel of the motor in the clearance, which is the real-time measurement value of the total width of the joint clearance. Online identification does not need to be completed once in each control cycle, and usually only needs to be measured once after detecting that the joint has crossed the clearance.

[0063] 104、Based on the relative position, relative velocity and total width of the joint clearance, calculate the collision time required for the motor of the joint to reach the boundary of the joint clearance;

[0064] The defect of the prior art is that the clearance is regarded as a position error, and the controller aims to eliminate the position error as soon as possible, resulting in the motor colliding with the connecting rod at a non-zero relative speed, at which time the kinetic energy carried by the motor is instantaneously released, forming the physical source of the impact. The controller can only passively suppress the impact after it occurs. In order to eliminate the impact source from the root, the embodiment selects to convert the collision itself from a transient event that cannot be controlled to a predictable process in the time dimension, thereby converting the dimension of the control problem from the spatial domain to the time domain, no longer passively responding to the position error, but actively predicting future collision events. Therefore, based on the relative position, relative velocity and total width of the joint clearance, the collision time required for the motor of the joint to reach the boundary of the joint clearance is calculated. According to the length of the collision time, the controller can actively intervene and manage the energy state before contact.

[0065] The physical meaning of the collision time is: the time needed for the motor to move from the current relative position to either side joint gap boundary, under the assumption that the current relative velocity remains unchanged. The controller first determines the direction of the relative velocity, which indicates which boundary the motor is currently moving towards, and accordingly the controller can determine the target boundary that will be collided. After determining the target boundary, the magnitude of the relative velocity represents the rate of approaching the target boundary, and in combination with the total width of the joint gap, the remaining distance from the current position to the target boundary can be calculated, and the time needed to travel the remaining distance can be calculated, which is the collision time.

[0066] 105. Adjusting impedance parameters in the impedance control law acting on the joint according to the collision time;

[0067] The impedance control law of the robot is a control strategy that defines the dynamic relationship between force and motion when the robot end interacts with the environment, and the goal is not to simply control the position or force, but to control the dynamic response characteristics of both. The core of this control strategy is to model the target dynamic behavior of the robot end as a virtual second-order mechanical system, i.e. a programmable spring-damper system. By setting the impedance parameters before the task or dynamically adjusting them during the task, the robot can exhibit high stiffness, high precision positioning characteristics, or low stiffness, high compliance force interaction characteristics in behavior, to adapt to different task requirements.

[0068] The mathematical form of the impedance control law is: τ_cmd=K(θd−θa)+D(θ˙d−θ˙a), where τ_cmd is the torque command calculated by the controller, K and D are impedance parameters, K represents virtual stiffness, and D represents virtual damping. (θd−θa) is the error between the desired position and the actual position, and (θ˙d−θ˙a) is the error between the desired velocity and the actual velocity. In this law, the stiffness term K(θd−θa) provides a restoring force proportional to the position error, like a virtual spring; the damping term D(θ˙d−θ˙a) provides a dissipative force proportional to the velocity error, like a virtual damper, to ensure the stability of the system and suppress oscillations.

[0069] The traditional impedance parameters K and D are usually fixed at a certain stage of the task, but in the process of crossing the joint gap, the dynamic characteristics required by the joint change dramatically: low resistance is needed in the middle of the joint gap to pass quickly, and high damping is needed when approaching the boundary to achieve buffering. A fixed impedance parameter cannot meet the two completely different requirements of efficient crossing of the joint gap and soft contact with the joint gap boundary. For example, in order to ensure the final positioning accuracy, the system needs high stiffness, but a high stiffness system will produce a large impact force at the moment of contact. Conversely, a low stiffness, high damping system designed for soft contact will be slow and inaccurate in normal trajectory tracking.

[0070] In this embodiment, impedance control traditionally used for adapting to external environment tasks is applied to the compliant control of the joint clearance of a robot, and the collision time is taken as a predictive time index. The impedance parameters in the impedance control law acting on the joint are adjusted through the collision time, so that the compliant behavior of the joint has foresight and adaptability. The specific adjustment logic of the impedance parameters is as follows: when the collision time is large, it indicates that the motor is moving freely in the joint clearance and is far away from any one side boundary, at this time, the control goal is to move efficiently and with low resistance. Therefore, the impedance parameters can all be set at a lower level, so that the motor moves in an extremely low viscous environment, avoiding unnecessary energy consumption and motion hysteresis. When the collision time decreases and tends to zero, it indicates that a collision is about to occur. At this time, the control goal is switched from efficient movement to impact-free contact. Physically, the key to achieving impact-free contact is to dissipate the relative kinetic energy before the collision occurs. At this time, the impedance parameters can be increased, which is equivalent to adding a virtual braking force before the motor hits the boundary, actively absorbing its kinetic energy. That is, the kinetic energy carried by the motor is dissipated in advance through the virtual damping before contact, thereby eliminating the impact source from the physical root.

[0071] 106、Based on the adjusted impedance parameters, calculate the torque command and drive the joint to move.

[0072] By applying the impedance control law, based on the dynamically adjusted impedance parameters, and combining the desired motion target with the real link state, the final torque command can be calculated. Specifically, first, the feedback control torque of the core is calculated based on the adjusted impedance parameters, that is, the adjusted impedance parameters are substituted into the impedance control law equation, and the actual joint state provided by the link encoder is taken as feedback, and through the calculation of position error and velocity error, the impedance torque component that realizes the expected compliant behavior is finally obtained. To improve tracking accuracy and response speed, pure feedback control is not enough, so a feedforward compensation torque is also introduced. The role of the feedforward compensation torque is to actively offset the predictable load force in the system before the error occurs, mainly including a static feedforward torque component for compensating the gravity of the joint itself and overcoming friction. This static feedforward torque component is calculated based on the known robot dynamics model. By superimposing this feedforward torque component and the aforementioned impedance torque component, the final torque command can be obtained. After the driver receives the torque command, it will accurately convert it into a current of corresponding size and apply it to the motor winding, thereby generating an electromagnetic torque proportional to the command value, driving the motor and the entire joint link to move, thus completing a complete closed-loop control cycle.

[0073] In practical applications, it is shown that: when the motor is about to cross the joint clearance and contact, the value of the collision time is very small, and the impedance parameter will be set to a very large value at this time. When the impedance parameter is substituted into the impedance control law, a braking torque opposite to the direction of relative motion will be generated. After this torque command is sent to the driver, the motor will be forced to slow down rapidly and smoothly before physical contact occurs. This controlled deceleration process is a physical manifestation of pre-dissipating impact energy before contact, and is the core action to achieve soft control.

[0074] In this embodiment, the position is not directly corrected, but the relative position and speed obtained by the state observer through real-time acquisition of double encoder data are used to calculate the collision time of the motor reaching the clearance boundary in real time. The predicted collision time is used as the basis to actively adjust the impedance parameter in the impedance control law, so as to effectively reduce the relative speed of the motor and the connecting rod before contact occurs. This enables the control system to change its dynamic behavior before the actual collision occurs, actively control the contact process to reduce the relative speed at the time of contact, thereby avoiding the occurrence of rigid collision, eliminating the impact source from the root, and achieving soft control of the robot joint clearance.

[0075] The control method for soft control of robot joint clearance provided in the present application will be described in detail below. Please refer to Figure 2 , Figure 2 Another embodiment of the control method for soft control of robot joint clearance provided in the present application is provided, and the method comprises:

[0076] 201. In each control cycle, real-time acquisition of motor-side encoder data and connecting rod-side encoder data of each joint of the robot is performed;

[0077] In this embodiment, step 201 is similar to step 101 of the foregoing embodiment, and will not be described here.

[0078] 202. Based on the preset kinematic state transition model of the joint, the relative state vector output in the last control cycle is recursively calculated to obtain a predicted vector in the current control cycle;

[0079] Step 202 is the first loop of the state observer, which aims to theoretically predict the current state of the robot based on physical laws before obtaining the actual measurement data of the current control cycle. The controller first calls the preset joint kinematics state transition model, which is a mathematical expression describing the natural evolution of the state vector over time. For example, the current position is the position at the last time plus the product of the velocity and the time interval. The controller takes the optimal estimate of the relative state vector output at the last control cycle (k-1) as input, substitutes it into the state transition model for one time step of recursive calculation, and obtains the predicted vector of the current control cycle (k). It should be noted that the predicted vector is purely based on theoretical model and historical data, and represents the prior estimate of the robot state without new external information.

[0080] 203、Fusing motor-side encoder data and link-side encoder data into system measurement value, calculating the difference between system measurement value and predicted vector;

[0081] After obtaining the theoretical prediction value, i.e., the predicted vector, the predicted vector needs to be compared with the real information obtained from the physical world, and the difference between the two needs to be quantified. This difference is the direct basis for subsequent state correction. First, the controller fuses the motor-side encoder data and link-side encoder data obtained from the dual encoder in real time in step 201 into the system measurement value of the current cycle, and then calculates the difference between the system measurement value and the predicted vector obtained in step 202 in the measurement dimension. This difference is called measurement residual or innovation in control theory. The calculation of this difference is not simply a numerical subtraction, but represents new information beyond the model prediction range in the real measurement data. If the residual is small, it means that the model prediction result is accurate; if the residual is large, it means that the robot state has changed in a way that the model cannot predict, and a large correction is needed.

[0082] 204、Combining the Kalman gain calculated by the state observer and the difference value to correct the predicted vector and obtain the optimal estimate of the relative state vector of the current control cycle;

[0083] The prediction vector of step 202 is completely based on the theoretical model, which may deviate from the true state due to model inaccuracy or unmodeled disturbances; while the system measurement of step 201, although from the physical world, inevitably contains sensor noise. Therefore, the purpose of step 204 is to correct the prediction vector obtained in step 202 using the difference calculated in step 203, so as to obtain the final optimal state estimation of the current control period. Specifically, the correction needs to be performed using the Kalman gain, which is a weight matrix dynamically calculated at each control period. It is used to optimally balance the proportion of model prediction and sensor measurement in the final state estimation, and its value depends on the evaluation of prediction uncertainty and measurement uncertainty.

[0084] The optimal estimation vector = the prediction vector + (the Kalman gain x the difference). When the measurement noise is large, the Kalman gain calculated by the state observer will be small, so that the influence of the correction term will be weakened, and the final estimation result will be more biased towards the prediction result of the theoretical model. When the model prediction error is large, the Kalman gain calculated by the state observer will be large, so that the influence of the correction term will be enhanced, and the final estimation result will be more biased towards adopting new measurement data to quickly track the actual changes. This correction process will simultaneously update all components in the relative state vector, i.e. the estimated values of the relative position, the relative velocity and the relative acceleration, and finally obtain the final output of the current control period, i.e. the optimal estimation relative state vector.

[0085] 205、Obtain the relative position and the relative velocity at the current time from the relative state vector, and obtain the total width of the joint clearance preset or identified online;

[0086] In this embodiment, step 205 is similar to step 103 of the foregoing embodiment, which will not be described here.

[0087] 206、If the direction of the relative velocity is towards any boundary of the joint clearance, calculate the remaining distance between the relative position and the boundary according to the total width of the joint clearance; divide the remaining distance by the absolute value of the relative velocity to obtain the collision time;

[0088] To ensure that the calculation of the collision time is only performed in the situation where a physical collision is possible, the controller first checks the direction of the relative velocity Δθ˙ obtained from the state vector, specifically by comparing the signs of the relative velocity and the relative position to determine the motion trend. If the signs of the two are the same, it means that the absolute value of the relative position is increasing, i.e. the motor is moving from the center of the joint gap towards the boundary. At this time, the remaining distance to the target boundary can be directly calculated according to the total width of the joint gap and the current relative position Δθ, and then the calculated remaining distance is divided by the absolute value of the relative velocity, and the result is the collision time. This collision time represents the time required to reach the physical contact boundary under the premise of maintaining the current relative velocity unchanged, and is a direct indicator of the urgency of the collision event.

[0089] 207、If the direction of the relative velocity is not towards either boundary of the joint gap or the relative velocity is zero, the collision time is set to a preset saturation value;

[0090] If the signs of the relative position and the relative velocity are opposite, it means that the direction of the relative velocity is not towards either boundary of the joint gap, which physically means that it is moving away from either boundary of the joint gap, and at this time there is no risk of collision. And if the relative velocity is zero, it means that the relative position between the motor and the link is constant at the current time, and at this time there is also no risk of collision. In these two cases, it is meaningless and illogical to calculate a finite collision time. Therefore, the collision time can be set to a preset saturation value, so that the controller explicitly does not need to start the subsequent compliant control.

[0091] 208、In each control cycle, the calculated collision time and the contact proximity are respectively substituted into the first function and the second function as inputs to calculate the target virtual damping value and the target virtual stiffness value;

[0092] In this embodiment, the virtual damping parameter is set as the first function, which is a continuous function with the collision time as the independent variable, and the function value of the first function is inversely proportional to the collision time. For example, the first function is an exponential function:

[0093] D_target = D_base + (D_max - D_base) · e −k_ttc·TTC ;

[0094] When the prediction time TTC is large, the exponential term approaches 0, and the target damping value approaches a very small base value D_base. When the prediction time TTC approaches 0, the exponential term approaches 1, and the target damping value increases smoothly and rapidly to the maximum value D_max.

[0095] In this embodiment, the virtual stiffness parameter is set as a second function, which is a continuous function with contact proximity as the independent variable. The function value of the second function is proportional to the contact proximity, which is a scalar from 0 to 1 used to describe the spatial position of the motor within the joint gap. The contact proximity is calculated based on the relative position and the total width of the joint gap. Specifically, the contact proximity is a normalized scalar from 0 to 1, used to describe the spatial position of the motor within the joint gap dimensionlessly. 0 represents the exact center of the gap, and 1 represents reaching the physical boundary on either side. For example, the second function is a polynomial function:

[0096] K_target=K_min+(K_max−K_min)·ρ m ;

[0097] ρ represents the contact proximity. When ρ is 0 (at the center of the gap), the target stiffness value is the minimum value K_min. When ρ is 1 (at the gap boundary), the target stiffness value smoothly increases to the maximum value K_max.

[0098] It should be noted that the specific mathematical forms of the first and second functions (such as exponential, polynomial, sigmoid functions, etc.) and their hyperparameters D_base, K_max, k_ttc, and m need to be designed and optimized based on the dynamic characteristics of the specific robot joints through simulation analysis and experimental data. The goal of their selection is to ensure that the parameter curves are sufficiently smooth and that the response characteristics meet expectations.

[0099] 209. Substitute the target virtual stiffness value, the target virtual damping value, the preset desired joint trajectory, and the data of the link-side encoder into the impedance control law to calculate the impedance torque component.

[0100] After calculating the target virtual stiffness and target virtual damping values, these values ​​are substituted into the impedance control law along with the preset desired joint trajectory and the data from the link-side encoder to calculate the impedance torque component. It should be noted that the calculated impedance torque component is an error-correcting torque, only effective when there is a deviation between the desired and actual states. The magnitude and direction of this impedance torque component depend not only on the magnitude of the error but also on the target virtual stiffness and target virtual damping values. Therefore, when the system requires compliant contact (large target virtual damping value), this impedance torque component will primarily exhibit a strong damping effect; when the system requires precise position maintenance (large target virtual stiffness value), this impedance torque component will primarily exhibit a strong position recovery capability.

[0101] 210. Obtain the static feedforward torque component used to compensate for gravity and friction, superimpose the impedance torque component and the static feedforward torque component to obtain the final torque command, and drive the joint movement.

[0102] Since the impedance torque acts as a feedback control, it only works after the error is generated. While the gravity and friction are persistent, if only feedback is used to compensate, it will inevitably lead to steady-state error. Therefore, in order to further improve the performance and accuracy on the basis of feedback control, the controller will also calculate the torque component for offsetting the predictable and state-dependent persistent external force according to the pre-established robot dynamics model, i.e. the static feedforward torque component. The static feedforward torque component usually includes: gravity compensation torque, which is calculated according to the current angles of all joints of the robot (i.e. the attitude) and the mass distribution of its connecting rods, to obtain the torque required to resist gravity; friction compensation torque, which is calculated according to the current motion speed of the joint and a preset friction force model (such as Coulomb friction + viscous friction model), to obtain the torque required to overcome the internal friction of the joint. Then, the impedance torque component calculated in step 209 is algebraically summed with the obtained static feedforward torque component, to obtain the final torque command.

[0103] In this embodiment, the energy is mainly dissipated by increasing the virtual damping D to achieve compliant contact. However, in some specific embodiments, an anti-thrust torque equal in size and opposite in direction to the inertia force of the motor itself can also be actively calculated. By applying this torque at the moment before the collision, the relative speed can theoretically be reduced to zero at the moment of contact. Specifically, when the collision time is less than a preset threshold, a transient feedforward torque component for actively offsetting the inertia of the motor is calculated based on the relative state vector and the collision time; the impedance torque component, the static feedforward torque component and the transient feedforward torque component are superimposed to obtain the final torque command, and the joint is driven to move. That is, this scheme will only be triggered when the collision time is less than a pre-set, extremely short time threshold, and the transient feedforward torque and the high-damping impedance torque are not mutually replaced, but work together. The feedforward term is responsible for the main inertia offset, while the damping term serves as a robust guarantee, responsible for absorbing any residual energy that the model fails to accurately describe, suppressing disturbances, and ensuring the absolute smoothness of the compliant control process.

[0104] The compliant control robot joint gap control system provided in the present application will be described in detail below. Please refer to Figure 3 , Figure 3 Another embodiment of the compliant control robot joint gap control system provided in the present application includes:

[0105] The first acquisition unit 301 is configured to acquire the motor-side encoder data and the connecting rod-side encoder data of each joint of the robot in real time at each control period;

[0106] The fusion unit 302 is configured to fuse the motor-side encoder data and the connecting rod-side encoder data by using a state observer to obtain a relative state vector of the joint, the relative state vector being composed of a relative position, a relative speed and a relative acceleration.

[0107] The second acquisition unit 303 is configured to acquire the relative position and the relative speed at the current moment from the relative state vector, and acquire the preset or online-identified total width of the joint clearance.

[0108] The first calculation unit 304 is configured to calculate, based on the relative position, the relative speed and the total width of the joint clearance, a collision time required for a motor of the joint to reach a boundary of the joint clearance.

[0109] The adjustment unit 305 is configured to adjust an impedance parameter in an impedance control law acting on the joint according to the collision time.

[0110] The second calculation unit 306 is configured to calculate a torque command based on the adjusted impedance parameter, and drive the joint to move.

[0111] In the system of the embodiment, the functions of the units correspond to the steps in the method embodiments described above, and thus will not be repeated here. Figure 1 or Figure 2 The steps in the method embodiments described above, and thus will not be repeated here.

[0112] The application also provides a control device for controlling joint clearance of a compliant control robot. Please refer to Figure 4 , Figure 4 An embodiment of the control device for controlling joint clearance of a compliant control robot provided by the application comprises:

[0113] A processor 401, a memory 402, an input and output unit 403 and a bus 404.

[0114] The processor 401 is connected with the memory 402, the input and output unit 403 and the bus 404.

[0115] The memory 402 stores a program, and the processor 401 invokes the program to execute any of the control methods for controlling joint clearance of a compliant control robot described above.

[0116] The application also relates to a computer readable storage medium, which stores a program. When the program runs on a computer, the computer executes any of the control methods for controlling joint clearance of a compliant control robot described above.

[0117] Those skilled in the art can clearly understand the specific working processes of the system, the device and the unit described above for the convenience and brevity of description. Please refer to the corresponding processes in the method embodiments described above, and thus will not be repeated here.

[0118] In several embodiments provided in the present application, it should be understood that the disclosed system, device and method can be implemented in other manners. For example, the described device embodiments are merely schematic. For example, the division of the units is only a logical function division. There can be another division manner for the actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections can be indirect couplings or communication connections through some interfaces, devices or units, and can be in electrical, mechanical or other forms.

[0119] The units described as separated components can or can not be physically separated, and the components displayed as units can or can not be physical units, i.e., can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purposes of the embodiments of the present application.

[0120] In addition, each functional unit in the embodiments of the present application can be integrated in one processing unit, or each unit can exist physically as a separate unit, or two or more units can be integrated in one unit. The integrated unit can be implemented in the form of hardware, or in the form of a software functional unit.

[0121] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer readable storage medium. Based on such an understanding, the technical solutions of the present application essentially or substantially, or all or part of the technical solutions, can be embodied in the form of a software product. The computer software product is stored in a storage medium, and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods in the embodiments of the present application. The foregoing storage medium includes: U disk, mobile hard disk, read-only memory (ROM, read-only memory), random access memory (RAM, random access memory), magnetic disk or optical disk, and various other media that can store program codes.

Claims

1. A method for controlling the joint clearance of a compliant robot, characterized in that, The control method includes: In each control cycle, the encoder data of the motor side and the encoder data of the link side of each joint of the robot are acquired in real time. The relative state vector of the joint is obtained by fusing the encoder data of the motor side and the encoder data of the link side using a state observer. The relative state vector consists of relative position, relative velocity and relative acceleration. Obtain the relative position and relative velocity at the current moment from the relative state vector, and obtain the preset or online identified total joint gap width; The collision time required for the motor of the joint to reach the joint gap boundary is calculated based on the relative position, the relative velocity, and the total width of the joint gap. The impedance parameters in the impedance control law acting on the joint are adjusted according to the collision time. The torque command is calculated based on the adjusted impedance parameters, and the joint is driven to move. The impedance parameters in the impedance control law include virtual damping parameters and virtual stiffness parameters. The virtual damping parameter is set as a first function, which is a continuous function with the collision time as the independent variable, and the function value of the first function is inversely proportional to the collision time. The virtual stiffness parameter is set as a second function, which is a continuous function with contact proximity as the independent variable. The function value of the second function is proportional to the contact proximity. The contact proximity is a scalar from 0 to 1 used to describe the spatial position of the motor in the joint gap. The contact proximity is calculated based on the relative position and the total width of the joint gap. The adjustment of the impedance parameter in the impedance control law acting on the joint according to the collision time includes: In each control cycle, the calculated collision time and contact proximity are substituted into the first function and the second function respectively as inputs to calculate the target virtual damping value and the target virtual stiffness value; The step of calculating the torque command based on the adjusted impedance parameters and driving the joint movement includes: Substitute the target virtual stiffness value, the target virtual damping value, the preset desired joint trajectory, and the link-side encoder data into the impedance control law to calculate the impedance torque component. Obtain the static feedforward torque component used to compensate for gravity and friction, and superimpose the impedance torque component and the static feedforward torque component to obtain the final torque command, and drive the joint movement; When the collision time is less than a preset threshold, the transient feedforward torque component for actively counteracting motor inertia is calculated based on the relative state vector and the collision time. The step of obtaining the static feedforward torque component for compensating for gravity and friction, superimposing the impedance torque component and the static feedforward torque component to obtain the final torque command, and driving the joint movement includes: The impedance torque component, the static feedforward torque component, and the transient feedforward torque component are superimposed to obtain the final torque command, which drives the joint movement.

2. The control method according to claim 1, characterized in that, The calculation of the collision time required for the motor of the joint to reach the joint gap boundary based on the relative velocity and the total width of the joint gap includes: If the direction of the relative velocity is toward any boundary of the joint space, then the remaining distance between the relative position and the boundary is calculated based on the total width of the joint space; The collision time is obtained by dividing the remaining distance by the absolute value of the relative velocity.

3. The control method according to claim 2, characterized in that, If the direction of the relative velocity is not toward any boundary of the joint gap or the relative velocity is zero, then the collision time is set to a preset saturation value.

4. The control method according to any one of claims 1 to 3, characterized in that, The process of fusing the motor-side encoder data and the link-side encoder data using a state observer to obtain the relative state vector of the joint includes: Based on the preset kinematic state transition model of the joint, the relative state vector output in the previous control cycle is recursively calculated to obtain the prediction vector for the current control cycle. The motor-side encoder data and the connecting rod-side encoder data are fused into a system measurement value, and the difference between the system measurement value and the prediction vector is calculated. By combining the Kalman gain calculated by the state observer with the difference, the prediction vector is corrected to obtain the optimal estimated relative state vector for the current control cycle.

5. A control system for compliantly controlling the joint clearance of a robot, characterized in that, The control system is used to execute the control method according to claim 1, and the control system includes: The first acquisition unit is used to acquire the motor-side encoder data and link-side encoder data of each joint of the robot in real time during each control cycle. The fusion unit is used to fuse the motor-side encoder data and the link-side encoder data using a state observer to obtain the relative state vector of the joint, which is composed of relative position, relative velocity and relative acceleration. The second acquisition unit is used to acquire the relative position and relative velocity at the current moment from the relative state vector, and to acquire the preset or online identified total width of the joint gap. The first calculation unit is used to calculate the collision time required for the motor of the joint to reach the boundary of the joint gap based on the relative position, the relative velocity and the total width of the joint gap; An adjustment unit is used to adjust the impedance parameters in the impedance control law acting on the joint according to the collision time. The second calculation unit is used to calculate the torque command based on the adjusted impedance parameters and drive the joint movement.

6. A control device for compliantly controlling the joint clearance of a robot, characterized in that, The control device includes: Processor, memory, input / output units, and bus; The processor is connected to the memory, the input / output unit, and the bus; The memory stores a program, which the processor invokes to execute the control method as described in any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium contains a program that, when executed on a computer, performs the control method as described in any one of claims 1 to 4.

Images