Control method, system and device for flexibly controlling joint gap of robot

Abstract

The invention discloses a control method, system and device for flexibly controlling a joint gap of a robot, which are used for radically eliminating an impact source and improving a flexible control effect on the joint gap of the robot. The method comprises the steps that in each control period, motor side encoder data and connecting rod side encoder data of all joints of the robot are obtained in real time; fusing the motor side encoder data and the connecting rod side encoder data by using a state observer to obtain a relative state vector of a joint; acquiring a relative position and a relative speed at the current moment from the relative state vector, and acquiring a preset or online identified total width of a joint gap; based on the relative position, the relative speed and the total width of the joint gap, calculating collision time required by a motor of the joint to reach the joint gap boundary; adjusting an impedance parameter in an impedance control law acting on the joint according to the collision time; and calculating a torque instruction based on the adjusted impedance parameter, and driving the joint to move.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, and in particular to a control method, system and device for compliantly controlling robot joint clearance. BACKGROUND

[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 is composed of a servo motor, a reducer (such as a harmonic reducer, an RV reducer), and a connecting rod and other components. In order to ensure mechanical transmission, a small gap must be left between the gears, which forms a nonlinear, discontinuous dead zone at the control level and is one of the most important performance limiting factors inherent in the robot servo control system.

[0003] In order to overcome the adverse effects of joint clearance, existing technologies mainly adopt compensation control strategies. The early approach is to compensate for the gap value based on offline calibration, and to perform open-loop fast position compensation when the motor command is reversed, i.e., to jump over the gap. A more advanced method introduces a state observer to estimate the actual position of the connecting rod by establishing a nonlinear model of the gap (such as a dead zone model, a hysteresis model), and designs a closed-loop controller for error correction. However, the essence of the existing technology is to regard the gap as a position error that must be eliminated as soon as possible, and to let the motor pass through this invalid gap area at the maximum acceleration and highest speed. This approach will cause a rigid collision between the driving end and the driven end after passing through the gap at a non-zero relative speed, thus inevitably exciting impact and vibration, which can only be passively suppressed after the impact occurs, and cannot fundamentally eliminate the impact source, affecting the compliant control effect of the joint clearance. SUMMARY

[0004] The present application provides a control method, system and device for compliantly controlling robot joint clearance, which can fundamentally eliminate the impact source and improve the compliant control effect of the robot joint clearance.

[0005] The first aspect of the present application provides a control method for compliantly controlling robot joint clearance, comprising: 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; Fusion of the motor-side encoder data and the connecting rod-side encoder data is performed by using a state observer to obtain a relative state vector of the joint, the relative state vector being composed of relative position, relative velocity and relative acceleration; The relative position and relative velocity at the current time are obtained from the relative state vector, and the total width of the joint clearance is obtained by presetting or online identification; calculating a collision time for the motor of the joint to reach a joint gap boundary based on the relative position, the relative velocity and the total joint gap width; adjusting an impedance parameter in an impedance control law acting on the joint according to the collision time; calculating a torque command based on the adjusted impedance parameter and driving the joint to move.

[0006] Optionally, the calculating a collision time for the motor of the joint to reach a joint gap boundary based on the relative velocity and the total joint gap width comprises: if a direction of the relative velocity is towards either boundary of the joint gap, calculating a remaining distance between the relative position and the boundary according to the total joint gap width; dividing the remaining distance by an absolute value of the relative velocity to obtain the collision time.

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

[0008] Optionally, the impedance parameter in the impedance control law comprises a virtual damping parameter and a virtual stiffness parameter; 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, a function value of the first function being in inverse proportion to the collision time; 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, a function value of the second function being in direct proportion to the contact proximity, the contact proximity being a scalar from 0 to 1 used to describe a spatial position of the motor within the joint gap, the contact proximity being calculated based on the relative position and the total joint gap width.

[0009] Optionally, the adjusting an impedance parameter in an impedance control law acting on the joint according to the collision time comprises: 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; the calculating a torque command based on the adjusted impedance parameter and driving the joint to move comprises: inputting the target virtual stiffness value, the target virtual damping value, a preset desired joint trajectory and the link side encoder data into the impedance control law to calculate an impedance torque component; The static feed-forward torque component for compensating gravity and friction is obtained, the impedance torque component and the static feed-forward torque component are superimposed to obtain a final torque instruction, and the joint motion is driven.

[0010] Optionally, the method further comprises: When the collision time is less than a preset threshold, a transient feed-forward torque component for actively canceling motor inertia is calculated based on the relative state vector and the collision time; The static feed-forward torque component for compensating gravity and friction is obtained, the impedance torque component and the static feed-forward torque component are superimposed to obtain a final torque instruction, and the joint motion is driven, comprising: The impedance torque component, the static feed-forward torque component and the transient feed-forward torque component are superimposed to obtain a final torque instruction, and the joint motion is driven.

[0011] Optionally, the relative state vector of the joint is obtained by fusing the motor-side encoder data and the link-side encoder data using a state observer, comprising: 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; The motor-side encoder data and the link-side encoder data are fused into system measurement values, and a difference between the system measurement values and the predicted vector is calculated; 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.

[0012] The second aspect of the present application provides a control system for controlling joint clearance of a compliant control robot, comprising: A first acquisition unit is configured to acquire motor-side encoder data and link-side encoder data of each joint of the robot in real time in each control period; A fusion unit is 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 relative position, relative velocity and relative acceleration; A second acquisition unit is configured to acquire the relative position and the relative velocity at the current time from the relative state vector, and acquire a total width of joint clearance identified in advance or online; A first calculation unit is configured to calculate a collision time required for a motor of the joint to reach a boundary of joint clearance based on the relative position, the relative velocity and the total width of joint clearance; an adjusting unit configured to adjust an impedance parameter in an impedance control law acting on the joint according to the collision time; a second calculating unit configured to calculate a torque command based on the adjusted impedance parameter and drive the joint to move.

[0013] The third aspect of the present application provides a control device for controlling the clearance of a robot joint in compliance control, which comprises: a processor, a memory, an input and output unit, and a bus; The processor is connected to the memory, the input and output unit, and the bus; The memory stores a program, and the processor invokes the program to execute the control method for controlling the clearance of a robot joint in compliance control according to the first aspect and any optional embodiment of the first aspect.

[0014] The fourth aspect of the present application provides a computer readable storage medium, which stores a program, and the program executes the control method for controlling the clearance of a robot joint in compliance control according to the first aspect and any optional embodiment of the first aspect when executed on a computer.

[0015] As can be seen from the above technical solutions, the present application has the following advantages: The present application does not directly correct the position, but obtains the relative position and relative speed obtained by the state observer through real-time acquisition of double encoder data, and calculates 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 that the relative speed of the motor and the connecting rod can be effectively reduced before contact occurs. This enables the control system to change its dynamic behavior in advance before the actual collision occurs, actively controls 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 compliance control of the clearance of the robot joint. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the present application, the following will briefly introduce the drawings needed in the embodiments. 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.

[0017] Figure 1 An embodiment flowchart of the control method for controlling the clearance of a robot joint in compliance control provided by the present application is shown in the figure; Figure 2 Another embodiment flowchart of the control method for controlling the clearance of a robot joint in compliance control provided by the present application is shown in the figure; Figure 3An embodiment structure schematic diagram of a control system for compliant control of robot joint clearance provided in the present application; Figure 4 An embodiment structure schematic diagram of a control device for compliant control of robot joint clearance provided in the present application. DETAILED DESCRIPTION

[0018] The present application provides a control method, system and device for compliant control of robot joint clearance, which is used to eliminate the impact source from the root and improve the compliant control effect of robot joint clearance.

[0019] Please refer to Figure 1 , Figure 1 An embodiment of a control method for compliant control of robot joint clearance provided in the present application, the method comprises: 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; The traditional robot joint is usually equipped with only one motor side encoder, but due to the existence of joint clearance, the motor rotation is not equal to the connecting rod rotation. When the motor idles in the joint clearance, the real state of the connecting rod cannot be known only by the motor encoder. Therefore, the embodiment needs to measure the motion state of both sides of the joint clearance in each control cycle, that is, to acquire the motor side encoder data (driving end) and the connecting rod side encoder data (driven end) of each joint of the robot. The motor side encoder data reflects the input, and the connecting rod 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.

[0020] 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 connecting rod 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 with the robot connecting rod. 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.

[0021] 102. 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, the relative state vector being composed of relative position, relative velocity and relative acceleration; The encoder data can only directly reflect the position information, but to achieve predictive control, the 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 an unusable result. Therefore, the state observer is used to fuse the motor side encoder data and the connecting rod side encoder data to extract a smooth, complete and accurate relative state vector reflecting the real dynamics from the original incomplete and noisy measurement data.

[0022] The relative state vector is used to completely describe the relative dynamic relationship between the joint driving end and the driven end at a certain moment, 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 connecting rod, which directly reflects the position in the gap; Δθ˙ is their relative velocity, which reflects the speed of approaching or moving away from the contact gap boundary; Δθ¨ is the relative acceleration, which indicates the trend of dynamic force change.

[0023] 103. Obtain the relative position and relative velocity at the current moment from the relative state vector, and obtain the preset or online identified total width of the joint gap; In order to calculate the collision time, in addition to obtaining the relative position and relative velocity at the current moment from the relative state vector, another key physical parameter, i.e. the width of the joint gap, is also needed. There are two ways to obtain the total width of the joint gap: Method one: During system initialization or offline calibration, the gap width of the joint 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 configuration file of the controller. In each control period, this preset value can be directly read from the configuration. This method is simple to implement, but it cannot adapt to changes in the gap due to wear or thermal expansion and contraction.

[0024] Method two: Through online identification, the estimation of the gap width is updated autonomously during the operation of the robot. 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 gap, which is the real-time measurement value of the total width of the joint gap. Online identification does not need to be completed once in every control period, and usually only needs to be measured once after detecting that the joint has crossed the gap.

[0025] 104. Calculate the collision time required for the motor of the joint to reach the boundary of the joint gap based on the relative position, relative velocity and total width of the joint gap; The drawback of existing technology is that it treats the gap as a positional error. The controller's goal is to eliminate this positional error as quickly as possible, causing the motor to collide with the connecting rod at a non-zero relative velocity. At this point, the kinetic energy carried by the motor is released instantaneously, forming the physical source of the impact. The controller can only passively suppress the impact after it occurs. To eliminate the impact source at its root, this embodiment transforms the collision itself from an instantaneous and uncontrollable event into a predictable process in the time dimension. This shifts the control problem from the spatial domain to the temporal domain, moving from passively responding to positional errors to actively predicting future collision events. Therefore, it is necessary to calculate the collision time required for the motor to reach the joint gap boundary based on the relative position, relative velocity, and total joint gap width. Depending on the collision time, the controller can actively intervene and manage the energy state before contact.

[0026] The physical meaning of collision time is: under the assumption that the current relative velocity remains constant, the time required for the motor to move from its current relative position to the boundary of the joint clearance on either side. The controller first determines the direction of the relative velocity, which indicates which boundary the motor is currently moving towards. Based on this, the controller can determine the target boundary where a collision is about to occur. After determining the target boundary, the magnitude of the relative velocity represents the speed at which the motor approaches that boundary. Combined with the total width of the joint clearance, the remaining distance from the current position to the target boundary is calculated, and the time required to travel this remaining distance is calculated; this time is the collision time.

[0027] 105. Adjust the impedance parameters in the impedance control law acting on the joint according to the collision time; Impedance control for robots is a control strategy that defines the dynamic relationship between force and motion when a robot's end effector interacts with its environment. Its goal is not simply to control position or force, but rather to control the dynamic response characteristics of both. The core of this control strategy is to model the target dynamic behavior of the robot's end effector as a virtual second-order mechanical system, namely a programmable spring-damper system. By setting impedance parameters before task execution or dynamically adjusting them during the task, the robot can exhibit either high-stiffness, high-precision positioning characteristics or low-stiffness, high-compliance force interaction characteristics to adapt to different task requirements.

[0028] The mathematical form of the impedance control law is τ cmd = K(θ d - θ a) + D(θ d - θ a), where τ cmd is the torque command output 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, and 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.

[0029] The traditional impedance parameters K and D are usually fixed at a certain stage of the task, but in the special process of crossing the joint clearance, the dynamic characteristics required by the joint change dramatically: low resistance is needed in the middle of the joint clearance to pass quickly, and high damping is needed near the boundary to achieve buffering. A fixed impedance parameter cannot simultaneously meet the two completely different requirements of efficient crossing of the joint clearance and soft contact with the joint clearance boundary. For example, 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 appear slow and inaccurate in normal trajectory tracking.

[0030] In this embodiment, the impedance control traditionally used for adapting to external environment tasks is applied to the soft control of the robot joint clearance, and the collision time is used as a predictive time indicator to adjust the impedance parameters in the impedance control law acting on the joint, so that the soft behavior of the joint is prospective and adaptive. 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 from any side boundary, so the control goal at this time is to move efficiently and with low resistance. Therefore, the impedance parameters can be set to a lower level, so that the motor moves in an extremely low viscous environment, avoiding unnecessary energy consumption and movement delay. When the collision time decreases and approaches 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 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 damping is increased before contact to pre-dissipate the kinetic energy carried by the motor through the virtual damping, thereby eliminating the source of impact from a physical root.

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

[0032] The impedance control law is applied, and based on the dynamically adjusted impedance parameters, in combination with the desired motion target and the actual 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 side encoder is used as feedback, and through calculation of the position error and the velocity error, the impedance torque component for realizing the desired compliance 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 for 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.

[0033] In actual application, it is shown that: when the motor is about to contact the joint clearance, the value of the collision time is very small, and at this time the impedance parameters are set to a very large value. When the impedance parameters are 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 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 of realizing compliance control.

[0034] In this embodiment, the position is not directly corrected, but the relative position and relative velocity 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. Based on the predicted collision time, the impedance parameters in the impedance control law are actively adjusted, so that the relative speed of the motor and the link can be effectively reduced before contact occurs. This enables the control system to change its dynamic behavior before the actual collision occurs, actively controls 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 realizing the compliance control of the joint clearance of the robot.

[0035] The compliance control method for 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 compliance control method for robot joint clearance provided in the present application comprises: 201、In each control cycle, real-time acquisition of motor-side encoder data and link-side encoder data of each joint of the robot is performed; In this embodiment, step 201 is similar to step 101 of the foregoing embodiment, and thus will not be described again here.

[0036] 202、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 a prediction vector in the current control cycle; Step 202 is the first loop of the state observer, and the purpose is to theoretically predict the current state of the robot based on physical laws before the actual measurement data in the current control cycle is obtained. The controller first calls the preset kinematic state transition model of the joint, 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 estimated relative state vector output in the previous control cycle (k-1) as input, substitutes it into the state transition model, and performs a time step of recursive calculation to obtain the prediction vector in the current control cycle (k). It should be noted that the prediction vector is purely based on theoretical model and historical data, and represents the priori estimation of the state of the robot without new external information.

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

[0038] 204、Combining the Kalman gain calculated by the state observer and the difference, the prediction vector is corrected to obtain the optimal estimated relative state vector in the current control cycle; 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 of step 202 with 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 made 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.

[0039] Optimal estimation vector = prediction vector + (Kalman gain x 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 estimation values of relative position, relative velocity and relative acceleration, and finally obtain the final output of the current control period, i.e. the optimal estimation relative state vector.

[0040] 205、From the relative state vector, obtain the relative position and relative velocity at the current time, and obtain the total width of the joint clearance preset or identified online; In this embodiment, step 205 is similar to step 103 of the foregoing embodiment, which will not be described here.

[0041] 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; In order 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 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 clearance to the boundary. At this time, the remaining distance to the target boundary can be directly calculated according to the total width of the joint clearance and the current relative position Δθ, and then the calculated remaining distance is divided by the absolute value of the relative velocity to obtain the collision time. The 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.

[0042] 207、if the direction of the relative velocity is not towards either boundary of the joint clearance or the relative velocity is zero, then set the time-to-collision as a pre-set saturation value; 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 clearance, which physically means that the motor is moving away from either boundary of the joint clearance, in which case 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, in which case there is also no risk of collision. In both cases, it is illogical to calculate a finite time-to-collision. Therefore, the time-to-collision can be set as a pre-set saturation value, so that the controller explicitly knows that there is no need to initiate the subsequent compliant control at the current time.

[0043] 208、in each control cycle, the calculated time-to-collision 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; In the present embodiment, the virtual damping parameter is set as the first function, which is a continuous function with the time-to-collision as the independent variable, and the function value of the first function is inversely proportional to the time-to-collision. For example, the first function is an exponential function: D_target=D_base+(D_max−D_base)·e −k_ttc·TTC ; When the predicted time TTC is large, the exponential term tends to 0, and the target damping value approaches a small base value D_base. When the predicted time TTC tends to 0, the exponential term tends to 1, and the target damping value smoothly and rapidly increases to the maximum value D_max.

[0044] In the present embodiment, the virtual stiffness parameter is set as the second function, which is a continuous function with the contact proximity as the independent variable, and the function value of the second function is proportional to the contact proximity, which is a scalar from 0 to 1 describing the spatial position of the motor within the joint clearance, and is calculated based on the relative position and the total width of the joint clearance. The contact proximity is a normalized scalar from 0 to 1, which is used to describe the spatial position of the motor within the joint clearance without dimension. 0 represents the center of the gap, and 1 represents the physical boundary of either side. For example, the second function is a polynomial function: K_target=K_min+(K_max−K_min)·ρ m ; ρ 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 boundary of the gap), the target stiffness value smoothly increases to the maximum value K_max.

[0045] 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.

[0046] 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. 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.

[0047] 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.

[0048] Since impedance torque is a feedback control mechanism, it only takes effect after an error occurs. Gravity and friction are continuous forces; relying solely on feedback for compensation would inevitably lead to steady-state errors. Therefore, to further improve performance and accuracy beyond feedback control, the controller calculates a static feedforward torque component based on a pre-established robot dynamics model to counteract predictable, state-dependent, continuous external forces. This static feedforward torque component typically includes: a gravity compensation torque, calculated based on the current angles (i.e., posture) of all robot joints and the mass distribution of its links to counteract gravity; and a friction compensation torque, calculated based on the current joint velocity and a pre-defined friction model (such as a Coulomb friction + viscous friction model) to overcome internal joint friction. The final torque command is obtained by algebraically summing the impedance torque component calculated in step 209 with the acquired static feedforward torque component.

[0049] In this embodiment, the energy is mainly dissipated by increasing the virtual damping D to achieve compliant contact. But in some specific embodiments, an anti-pushing torque equal in size and opposite in direction to the inertial 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, based on the relative state vector and the collision time, a transient feedforward torque component for actively canceling the inertia of the motor is calculated; 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 cancellation, 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.

[0050] 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 comprises: A first acquisition unit 301 is configured to acquire, in each control cycle, motor-side encoder data and linkage-side encoder data of each joint of the robot in real time; A fusion unit 302 is configured to fuse the motor-side encoder data and the linkage-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 velocity and a relative acceleration; A second acquisition unit 303 is configured to acquire the relative position and the relative velocity at the current moment from the relative state vector, and acquire a total width of the joint gap which is preset or identified online; A first calculation unit 304 is configured to calculate a collision time required for the motor of the joint to reach a boundary of the joint gap based on the relative position, the relative velocity and the total width of the joint gap; An adjustment unit 305 is configured to adjust an impedance parameter in an impedance control law acting on the joint according to the collision time; A second calculation unit 306 is configured to calculate a torque command based on the adjusted impedance parameter, and drive the joint to move.

[0051] In the system of this embodiment, the functions of each unit correspond to the steps in the method embodiments shown in the foregoing Figure 1 or Figure 2 , which will not be described here again.

[0052] The present application also provides a compliant control robot joint gap control device, please refer toFigure 4 , Figure 4 An embodiment of the control device for controlling the joint gap of a compliant control robot provided in the present application comprises: a processor 401, a memory 402, an input output unit 403, a bus 404; The processor 401 is connected with the memory 402, the input output unit 403 and the bus 404; The memory 402 stores a program, and the processor 401 invokes the program to execute any of the above control methods for controlling the joint gap of a compliant control robot.

[0053] The present application also relates to a computer readable storage medium, which stores a program, and when the program runs on a computer, the computer executes any of the above control methods for controlling the joint gap of a compliant control robot.

[0054] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the above-described system, device and unit can refer to the corresponding process in the foregoing method embodiments, which will not be repeated here.

[0055] In several embodiments provided in the present application, it should be understood that the disclosed system, device and method can be implemented in other ways. For example, the above-described device embodiments are only schematic, for example, the division of units is only a logical function division, and actual implementation can have another division manner, 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 coupling or direct coupling or communication connection between the units shown or discussed can be indirect coupling or communication connection through some interface, device or unit, and can be electrical, mechanical or other forms.

[0056] The units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, that is, they can be located in one place, or can be distributed on a plurality of network units. According to actual needs, some or all of the units can be selected to achieve the purpose of the embodiment.

[0057] In addition, each functional unit in each embodiment of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit. The above integrated unit can be realized in the form of hardware or in the form of software functional unit.

[0058] The integrated unit, if implemented in the form of a software function unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on such understanding, the technical solutions of the present application, essentially or in other words, the part that contributes to the prior art or the whole 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, including a number of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the various embodiments of the method of the present application. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, read-only memory), a random access memory (RAM, random access memory), a magnetic disk or an optical disk, and various media that can store program codes.

Claims

1. A control method of controlling a joint gap of a compliant robot, characterized by, The control method comprises: In each control cycle, real-time acquisition of motor-side encoder data and link-side encoder data of each joint of the robot; Fusion of the motor-side encoder data and the link-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 velocity and a relative acceleration; Acquisition of the relative position and the relative velocity at the current moment from the relative state vector, and acquisition of a total joint clearance width which is preset or identified online; Calculation of a collision time required for the motor of the joint to reach a joint clearance boundary based on the relative position, the relative velocity and the total joint clearance width; Adjustment of an impedance parameter in an impedance control law acting on the joint according to the collision time; Calculation of a torque command based on the adjusted impedance parameter, and driving of the joint to move.

2. The control method according to claim 1, characterized by, The calculation of 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: If the direction of the relative velocity is towards any boundary of the joint clearance, then the remaining distance between the relative position and the boundary is calculated according to the total joint clearance width; The remaining distance is divided by the absolute value of the relative velocity to obtain the collision time.

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

4. The control method according to claim 1, characterized by, The impedance parameter in the impedance control law comprises a virtual damping parameter and a virtual stiffness parameter; The virtual damping parameter is set as a first function, the first function being a continuous function with the collision time as the independent variable, and the function value of the first function being inversely proportional to the collision time; The virtual stiffness parameter is set as a second function, the second function being a continuous function with contact proximity as the independent variable, and the function value of the second function being proportional to the contact proximity, the contact proximity being a scalar from 0 to 1 used to describe the 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.

5. The control method according to claim 4, characterized by The adjustment of the impedance parameter in the impedance control law acting on the joint according to the collision time comprises: 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 a target virtual damping value and a target virtual stiffness value. The calculation of the torque command based on the adjusted impedance parameter, and the driving of the joint to move, comprises: The target virtual stiffness value, the target virtual damping value, a preset desired joint trajectory and the link-side encoder data are substituted into the impedance control law to calculate an impedance torque component; A static feedforward torque component for compensating for gravity and friction is acquired, the impedance torque component and the static feedforward torque component are superimposed to obtain a final torque command, and the joint is driven to move.

6. The control method according to claim 5, characterized by The method further comprises: 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; 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: 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.

7. The control method according to any one of claims 1 to 6, characterized by, The motor-side encoder data and the link-side encoder data are fused by the state observer to obtain the relative state vector of the joint, including: Based on a preset kinematic state transition model of the joint, the relative state vector output in the last control period is recursively calculated to obtain a predicted vector in the current control period; The motor-side encoder data and the link-side encoder data are fused into system measurement values, and the difference between the system measurement values and the predicted vector is calculated; The predicted vector is corrected by combining the Kalman gain calculated by the state observer and the difference to obtain an optimal estimated relative state vector in the current control period.

8. A control system for compliantly controlling a joint gap of a robot, characterized by The control system comprises: A first acquisition unit is configured to acquire motor-side encoder data and link-side encoder data of each joint of a robot in real time in each control period; A fusion unit is configured to fuse the motor-side encoder data and the link-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 velocity and a relative acceleration; A second acquisition unit is configured to acquire the relative position and the relative velocity at the current time from the relative state vector, and acquire a total joint clearance width identified in advance or online; A first calculation unit is 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; An adjustment unit is configured to adjust an impedance parameter in an impedance control law acting on the joint according to the collision time; A second calculation unit is configured to calculate a torque instruction based on the adjusted impedance parameter and drive the joint motion.

9. A control device for controlling a joint gap of a compliant robot, characterized by The control device comprises: A processor, a memory, an input / output unit and a bus; The processor is connected with the memory, the input / output unit and the bus; The memory stores a program, and the processor invokes the program to execute the control method in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a program, and the program executes the control method in any one of claims 1 to 7 when executed on a computer.

Images