Contour error compensation method and device of robot and robot system

Abstract

The invention discloses a contour error compensation method and device for a robot, a robot system, a storage medium and a computer program product, and the method comprises the steps: determining a contour error estimation value of a tail end according to the joint angle information of each joint of a multi-joint robot, and inputting the contour error estimation value into a preset PI controller, the contour error estimation value is controlled through a PI controller, a PI control output value is obtained, the PI controller output value is multiplied by the compensation distribution coefficient corresponding to each joint, a joint speed compensation value corresponding to each joint is obtained, the joint speed compensation value corresponding to each joint is applied to the corresponding joint, and the joint speed compensation value corresponding to each joint is obtained. Therefore, the motion of each joint of the multi-joint robot is corrected and compensated. According to the scheme, the contour error of the multi-joint robot is effectively reduced, and the tail end track precision and the motion stability are improved.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of robot technology, and particularly relates to a robot contour error compensation method and device, a robot system, a storage medium and a computer program product. BACKGROUND

[0002] Multi-joint robots are widely used in assembly, welding, detection and other scenarios due to their flexible motion capabilities. Such applications usually require a specific point (such as a welding gun nozzle or a detection probe tip) on the robot end or tool to run strictly according to a preset straight line trajectory to ensure work precision. However, in actual motion processes, the straight line trajectory of the robot end often deviates due to the complexity of multi-joint coordinated motion, eventually forming a contour error, i.e., the distance between the actual running trajectory and the expected trajectory of the end, which directly leads to a decrease in work precision, and even causes product scrap, equipment collision and other problems.

[0003] The generation of contour error is mainly due to the inherent characteristics of the multi-joint robot servo control system and external disturbances: on the one hand, the servo loop of each joint is affected by mechanical characteristics such as mechanical transmission gap, motor dynamic response difference, joint friction damping variation, as well as electrical disturbances such as electrical signal transmission delay and power drive fluctuation, which will inevitably cause joint following deviation; on the other hand, when multi-joint coordinated motion is performed, the motion states of each joint are coupled, and dynamic loads such as centrifugal force and Coriolis force will further amplify the following deviation of a single joint, and these deviations will be transmitted to the end through the kinematics of the robot, causing the end trajectory to deviate from the expected straight line and forming a significant contour error.

[0004] To reduce contour error, existing technologies mainly optimize in two directions:

[0005] One is to improve single-axis servo control algorithms (such as introducing anti-disturbance controllers and response speed optimization algorithms) to improve single-axis following performance. In theory, if there is no deviation in single-axis following, the end trajectory can completely follow the expected straight line. However, in actual working conditions, dynamic loads (such as centrifugal force and Coriolis force) generated by multi-joint coordinated motion cannot be completely offset by single-axis control, and single-axis following deviation always exists, which cannot fundamentally eliminate contour error.

[0006] Secondly, a cross-coupling control strategy is adopted, and the contour error compensation amount is distributed to each motion axis to correct the deviation by establishing a contour error estimation model. However, the existing cross-coupling control related schemes are all designed for numerical control machine tools, and in the application scenarios of the machine tools, the motion directions of each motion axis (such as the X axis and the Y axis) are fixed and perpendicular to each other, and the compensation amount can be directly distributed according to the coordinate axis direction. The motion of the multi-joint robot depends on the rotation of each joint, and the motion direction of the joint changes in real time with the robot posture. The existing cross-coupling control for machine tools cannot solve the problem of dynamic distribution of compensation coefficients in the multi-joint robot scenario, which makes it difficult to directly adapt to the multi-joint robot and cannot effectively correct the end contour error.

[0007] In summary, for the current multi-joint robot linear trajectory control, there is still the problem of end contour error caused by mechanical or electrical disturbance and multi-joint coupled load.

[0008] The above content is only used to assist in understanding the technical solutions of the present application and does not represent the acknowledgement of the above content as prior art. SUMMARY

[0009] The present application aims to provide a robot contour error compensation method, device, robot system, storage medium and computer program product, to solve the problem of end contour error of multi-joint robot caused by mechanical or electrical disturbance and multi-joint coupled load in related solutions, to achieve the effect of effectively reducing the contour error of multi-joint robot and improving the end trajectory precision and motion stability by estimating the end contour error of robot, PI control and combining with compensation distribution coefficient to distribute joint speed compensation value and correct and compensate the joint motion.

[0010] The present application provides a robot contour error compensation method, the robot is a multi-joint robot, and the method comprises the following steps: acquiring joint angle information at each joint of the multi-joint robot; determining a contour error estimation value of the end of the multi-joint robot according to the joint angle information; inputting the contour error estimation value into a preset PI controller, controlling the contour error estimation value through the PI controller to obtain a PI controller output value; multiplying the PI controller output value by a compensation distribution coefficient corresponding to each joint respectively to obtain a joint speed compensation value corresponding to each joint; and applying the joint speed compensation value corresponding to each joint to the corresponding joint respectively to correct and compensate the motion of each joint of the multi-joint robot.

[0011] In some embodiments, determining the contour error estimation of the multi-joint robot end based on the joint angle information comprises: performing inverse kinematics calculation on the joint angle information of each joint to obtain actual position coordinates of the multi-joint robot end; obtaining expected position coordinates of the multi-joint robot end on a preset motion trajectory; and calculating the contour error estimation based on a deviation relationship between the actual position coordinates and the expected position coordinates.

[0012] In some embodiments, the contour error estimation is calculated based on a deviation relationship between the actual position coordinates and the expected position coordinates, comprising: calculating a first deviation value in an X direction and a second deviation value in a Y direction of the actual position coordinates and the expected position coordinates, respectively; and deriving the contour error estimation based on the first deviation value, the second deviation value, and straight line parameters of the preset motion trajectory through geometric relationship.

[0013] In some embodiments, the first deviation value E x is calculated as follows: E x = |X R -X P |; and the second deviation value E Y is calculated as follows: E Y = |Y R -Y P |; wherein X R is an X-axis component of the expected position coordinates, X P is an X-axis component of the actual position coordinates, Y R is a Y-axis component of the expected position coordinates, and Y P is a Y-axis component of the actual position coordinates.

[0014] In some embodiments, a compensation distribution coefficient corresponding to each joint is calculated based on an included angle relationship between a unit vector of a joint rotation tangent direction of the multi-joint robot in a current posture and a direction of the contour error estimation, and an included angle relationship between unit vectors of rotation tangent directions of adjacent joints.

[0015] In some embodiments, when the multi-joint robot is a two-joint robot, a calculation formula of a compensation distribution coefficient α corresponding to a first joint is as follows: a calculation formula of a compensation distribution coefficient β corresponding to a second joint is as follows: wherein E c is the contour error estimation, θ1 is an included angle between a unit vector of a first joint rotation tangent direction and a direction of the contour error estimation, θ2 is an included angle between a unit vector of a second joint rotation tangent direction and the direction of the contour error estimation, and Φ is an included angle between the unit vectors of the first joint rotation tangent direction and the second joint rotation tangent direction.

[0016] According to the above method, another aspect of the present application provides a robot contour error compensation device, comprising: an angle acquisition unit configured to obtain joint angle information at each joint of the multi-joint robot; an error calculation unit configured to determine a contour error estimation value of the end of the multi-joint robot according to the joint angle information; a PI control unit configured to input the contour error estimation value into a preset PI controller, control the contour error estimation value through the PI controller, and obtain a PI control output value; a compensation distribution unit configured to multiply the PI controller output value by a compensation distribution coefficient corresponding to each joint respectively, and obtain a joint speed compensation value corresponding to each joint; and a compensation execution unit configured to apply the joint speed compensation value corresponding to each joint to the corresponding joint respectively to correct and compensate the movement of each joint of the multi-joint robot.

[0017] According to the above device, still another aspect of the present application provides a robot system, comprising: the above-mentioned robot contour error compensation device.

[0018] According to the above method, still another aspect of the present application provides a storage medium, comprising a stored program, wherein the device where the storage medium is located executes the above-mentioned robot contour error compensation method when the program runs.

[0019] According to the above method, still another aspect of the present application provides a computer program product, comprising a computer program, which realizes the steps of the above-mentioned robot contour error compensation method when executed by a processor.

[0020] According to the above method, still another aspect of the present application provides a computer program product, comprising a computer program, which realizes the steps of the above-mentioned robot contour error compensation method when executed by a processor.

[0021] Other features and advantages of the present application will be described in the following description, and become apparent from the description, or be learned through the practice of the present application.

[0022] The technical solutions of the present application will be further described in detail below with the aid of drawings and examples. BRIEF DESCRIPTION OF DRAWINGS

[0023] Figure 1 A flowchart of an embodiment of the contour error compensation method of the robot of the present application;

[0024] Figure 2 A structural diagram of an embodiment of the contour error compensation device of the robot of the present application;

[0025] Figure 3 A cross-coupling controller and a contour error estimation block diagram;

[0026] Figure 4 A follow trajectory deviation diagram;

[0027] Figure 5 A contour error calculation diagram;

[0028] Figure 6 A joint compensation distribution coefficient calculation diagram;

[0029] Figure 7 A cross-coupling controller block diagram.

[0030] In combination with the accompanying drawings, the reference signs in the embodiments of the present application are as follows:

[0031] 101 - angle acquisition unit; 102 - error calculation unit; 103 - PI control unit; 104 - compensation distribution unit; 105 - compensation execution unit. DETAILED DESCRIPTION

[0032] To make the objectives, technical solutions and advantages of the present application clearer, the technical solutions of the present application will be described below in combination with specific embodiments of the present application and corresponding drawings. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work fall within the scope of protection of the present application.

[0033] According to an embodiment of the present application, a contour error compensation method of a robot is provided, the robot being a multi-joint robot, the multi-joint robot referring to an automated device having two or more joints that can rotate or move independently, and being able to drive an end effector (such as a probe or a welding gun) to complete a preset trajectory movement through joint cooperative movement, which is different from a single-joint robot or a machine tool with fixed movement axis, and the joint movement direction of which changes in real time with the posture. For example, Figure 1 A flowchart of an embodiment of the method of the present application is shown. The contour error compensation method of the robot can include steps S110 to S150.

[0034] At step S110, joint angle information at each joint of the multi-joint robot is acquired.

[0035] The joint angle information refers to real-time data of the current rotation angle of each joint of the multi-joint robot during movement. The position of the end of the multi-joint robot is uniquely determined by the joint angles, and only by acquiring the real-time angles of each joint can the actual position of the end be deduced. If the angle information of any joint is missing, the actual position of the end cannot be accurately calculated, and thus it is impossible to determine whether the trajectory deviates from the preset path.

[0036] Each joint of the multi-joint robot is configured with an angle detection device (such as an optical encoder), which operates synchronously with the joint motor and collects real-time joint rotation angle data.

[0037] At step S120, a contour error estimation value of the end of the multi-joint robot is determined according to the joint angle information.

[0038] The contour error is a core problem of the straight-line trajectory movement of the multi-joint robot, and only by quantifying the error can a compensating strategy be designed.

[0039] In some embodiments, the specific process of determining the contour error estimation value of the end of the multi-joint robot according to the joint angle information in step S120 includes steps S210 to S230.

[0040] Step S210, inverse solution calculation is performed on the joint angle information of each joint to obtain the actual position coordinates of the end of the multi-joint robot.

[0041] The joint angle itself cannot directly reflect whether the end deviates from the preset trajectory, and only by converting the abstract joint angle into intuitive end position coordinates can the deviation from the coordinates on the expected trajectory be determined.

[0042] Inverse solution calculation is a calculation process of deducing the actual position coordinates of the end effector in the spatial coordinate system based on the kinematic model of the multi-joint robot, through known joint angle information. The actual position coordinates of the end are the specific coordinate values of the end effector in the preset spatial coordinate system at the current movement time, reflecting the real spatial position of the end.

[0043] Specifically, first, a kinematic inverse solution model of the robot is established based on the mechanical structure parameters of the multi-joint robot; during the movement of the robot, real-time angle information of each joint is collected, and then the angle information of all joints is substituted into the pre-established kinematic inverse solution model to obtain the X-axis coordinate component and the Y-axis coordinate component of the end effector in the preset spatial coordinate system through calculation, and finally the complete actual position coordinates of the end are formed.

[0044] Step S220, obtain the expected position coordinate of the multi-joint robot end on the preset motion trajectory.

[0045] Step S230, based on the deviation relationship between the actual position coordinate and the expected position coordinate, calculate the contour error estimation value.

[0046] The expected position coordinate is an ideal coordinate value that the end should reach on the preset motion trajectory corresponding to the current motion time, which is extracted from the parameter library of the preset motion trajectory.

[0047] When the robot runs on a straight line trajectory, due to the following deviation of each joint motor, the final end position will deviate under the joint action. As shown in Figure 4 , the solid line is the preset motion trajectory, the dashed line is the actual motion trajectory, V1 and V2 are unit vectors in the tangential direction of the two joints, the actual position of the moving end is P point, and the expected position is R point. The coordinates corresponding to P point and R point are shown in Figure 5 .

[0048] The deviation relationship between the actual position coordinate and the expected position coordinate includes two forms of axial difference and geometric distance, where the axial difference can only reflect the deviation in a single direction and cannot directly reflect the overall deviation of the end to the preset trajectory. The definition of contour error is the shortest distance from the actual position of the end to the preset trajectory, so the geometric distance must be derived based on the deviation relationship to accurately quantify the deviation degree of the end trajectory.

[0049] In some embodiments, in step S230, based on the deviation relationship between the actual position coordinate and the expected position coordinate, the specific process of calculating the contour error estimation value includes steps S310 and S320.

[0050] Step S310, respectively calculate the first deviation value of the actual position coordinate and the expected position coordinate in the X direction and the second deviation value in the Y direction.

[0051] The spatial position of the multi-joint robot end can be completely described by the X-axis and Y-axis components of the rectangular coordinate system, and the deviation of the end to the preset straight line trajectory is essentially the comprehensive result of the actual position deviating from the expected position in the X and Y directions. Therefore, first calculating the deviation values in the X and Y directions can simplify the complexity of subsequent geometric derivation. If the error in the Z direction needs to be calculated, the deviation value of the actual position coordinate and the expected position coordinate in the Z direction also needs to be calculated.

[0052] The first deviation value refers to the difference between the actual position coordinates of the end effector of the multi-joint robot and the expected position coordinates on the preset motion trajectory in the X-axis direction, and the first deviation value refers to the difference between the actual position coordinates of the end effector of the multi-joint robot and the expected position coordinates on the preset motion trajectory in the Y-axis direction.

[0053] The first deviation value E x The formula for calculation is:

[0054] E x =|X R -X P |;

[0055] The second deviation value E Y The formula for calculation is:

[0056] E Y =|Y R -Y P |;

[0057] Among them, X R X represents the X-axis component of the desired position coordinates. P The X-axis component of the actual position coordinates, Y R Y is the Y-axis component of the desired position coordinates. P The Y-axis component represents the actual position coordinates.

[0058] In the motion control of multi-joint robots, the deviation of the end effector in the X and Y directions is a fundamental factor causing contour errors. By calculating the absolute values ​​of these deviations, the degree of deviation of the end effector in each axis can be accurately and intuitively obtained. Absolute values ​​are used because the deviation value only needs to reflect the magnitude of the deviation, without needing to consider the specific direction of deviation. This simplifies the complexity of subsequent contour error calculations and ensures that the deviation value is non-negative, meeting the basic requirement of quantitatively assessing the degree of deviation. If absolute values ​​are not used, the deviation value may be positive or negative, which can easily lead to distorted contour error calculation results due to sign issues in subsequent calculations, failing to accurately reflect the actual degree of deviation.

[0059] Step S320: Based on the first deviation value, the second deviation value, and the linear parameters of the preset motion trajectory, the estimated value of the contour error is derived through geometric relationships.

[0060] like Figure 5 As shown, when the preset direction angle of the straight trajectory is θ, the shortest distance from the actual position of the end point to the straight line (profile error E) is... c (This can be achieved through X-direction deviation E) x , Y direction deviation E y The trigonometric function relationship with angle θ is directly derived. Therefore, the formula for calculating the estimated contour error is:

[0061] E c = -E x sinθ + E y cosθ;

[0062] wherein θ is an angle between the preset straight line trajectory and the positive direction of the X axis.

[0063] By calculating the X / Y axis deviation first and then combining the straight line parameters to calculate the shortest distance, the quantization distortion caused by replacing the profile error with a single axis deviation is solved, and the error calculation difficulty in the complex trajectory scenario is reduced.

[0064] At step S130, the profile error estimate value is input into a preset PI controller, and the profile error estimate value is controlled by the PI controller to obtain a PI control output value.

[0065] The profile error estimate value is a deviation, which needs to be converted into a correction amount by a controller. The PI controller has the characteristics of fast response of proportional control and elimination of static error of integral control, and can effectively convert the profile error into a stable control signal.

[0066] Specifically, the calculated profile error estimate value is taken as an input signal and transmitted to a preset PI controller; the PI controller operates the input error signal according to a preset proportional coefficient (P) and an integral coefficient (I): the proportional element outputs an immediate control amount according to the current error size, the integral element outputs a compensation control amount according to the accumulated amount of error, and the two are superimposed to form a PI control output value; the PI controller outputs the value.

[0067] At step S140, the PI controller output value is multiplied by a compensation distribution coefficient corresponding to each joint, respectively, to obtain a joint speed compensation value corresponding to each joint.

[0068] Different joints of the multi-joint robot have different contributions to the end profile error (for example, joints close to the end have a greater impact on the end position, and joints far from the end have a smaller impact), and if the PI control output value is evenly distributed to all joints, it will cause some joints to be overcompensated (causing new errors) and some joints to be undercompensated (unable to eliminate the original error). The compensation distribution coefficient is used to distribute the compensation amount according to the joint contribution, so as to ensure that the compensation value of each joint matches its influence on the error.

[0069] In some embodiments, the compensation distribution coefficient corresponding to each joint is calculated based on the angle relationship between the unit vector of the tangent direction of the rotation of each joint under the current posture of the multi-joint robot and the direction of the profile error estimate value, and the angle relationship between the unit vectors of the tangent directions of the rotations of adjacent joints.

[0070] Each joint of a multi-joint robot has a different ability to correct end-effector contour errors: if the joint's rotation direction is consistent with the contour error direction, even a small movement of that joint can significantly correct the error; if the joint's rotation direction is perpendicular to the contour error direction, the joint's movement contributes very little to error correction. Furthermore, the movement directions of adjacent joints may be coupled, requiring adjustments to the compensation allocation based on the angular relationship between adjacent joints to avoid mutual cancellation of compensation amounts. If these angular relationships are not considered and a fixed coefficient is used to allocate compensation, some joints may experience undercompensation or overcompensation.

[0071] The robot's current posture refers to the overall spatial form formed by the joints of a multi-joint robot through specific angle combinations during motion. This form determines the position of the end effector and the influence weight of each joint on the end effector's motion, and changes dynamically with changes in joint angles. The unit vector along the joint rotation tangent direction refers to a unit-length vector perpendicular to the joint rotation axis and along the joint rotation tangent direction (e.g., ...). Figure 6 V1 and V2 in the equation describe the direction of the linkage motion during joint rotation, and their direction is determined by the joint rotation direction and the robot's structural parameters. The direction of the contour error estimate refers to the direction from the actual position of the end effector of the multi-joint robot to the shortest distance of the preset motion trajectory; that is, the direction perpendicular to the preset trajectory and pointing towards it. This direction reflects the target direction that the end effector needs to correct. The angle between the unit vector of the joint rotation tangent direction and the direction of the contour error estimate is... Figure 6 θ v1 θ v2 The angle between the unit vectors of the rotational tangent directions of adjacent joints is... Figure 6 The angle between V1 and V2.

[0072] Specifically, firstly, the current posture of the multi-joint robot is determined: by collecting real-time angle information of each joint, the rotational state and overall structural shape of each joint are clarified; secondly, the unit vector of the rotational tangent direction of each joint is calculated: based on the robot's mechanical structural parameters and the current joint angle, the unit vector of the tangent direction when each joint rotates is derived; then, the direction of the contour error estimate is determined, and based on the contour error, the vertical direction vector pointing from the actual position of the end effector to the preset trajectory is determined and standardized as a unit vector; next, two types of angles are calculated: one is the angle between the tangent unit vector of each joint and the contour error direction unit vector, and the other is the angle between the tangent unit vectors of adjacent joints; finally, these angles are substituted into the preset coefficient calculation formula to obtain the compensation allocation coefficient corresponding to each joint, thus completing the dynamic calculation of the coefficients.

[0073] The compensation distribution coefficient is calculated by an angle relationship, so that the joint speed compensation value is directly linked to the correction ability of the joint pair error, and the joint that contributes more to error correction is allocated more compensation, and the joint that contributes less is allocated less compensation, avoiding resource waste or insufficient correction caused by average allocation, and significantly improving the correction efficiency of the contour error, so that the end trajectory converges to the preset path faster.

[0074] In some embodiments, when the multi-joint robot is a two-joint robot, the calculation formula of the compensation distribution coefficient α corresponding to the first joint is:

[0075]

[0076] The calculation formula of the compensation distribution coefficient β corresponding to the second joint is:

[0077]

[0078] wherein E c is the contour error estimation value, θ1 is the angle between the unit vector of the tangential direction of rotation of the first joint and the direction of the contour error estimation value (e.g. the angle θ1 between V1 and Ec in FIG. 1), Figure 6 v1 θ2 is the angle between the unit vector of the tangential direction of rotation of the second joint and the direction of the contour error estimation value (e.g. the angle θ2 between V2 and Ec in FIG. 1), Figure 6 v2 and Φ is the angle between the unit vectors of the tangential directions of rotation of the first joint and the second joint.

[0079] For the structural characteristics of the two-joint robot, the coupling relationship between the joints is quantified by introducing the Φ parameter, solving the problem of weakened compensation effect caused by ignoring coupling. For example, when the motion directions of the two joints are opposite (Φ is close to 180°), the formula will automatically adjust the ratio of α and β, avoid the mutual offset of the compensation amount, and make the compensation of the two joints work together, significantly improving the correction efficiency of the end contour error.

[0080] In some embodiments, after the PI controller output value is multiplied by the compensation distribution coefficient corresponding to each joint, the result is multiplied by a compensation gain coefficient K to obtain the compensation value of each joint. The compensation gain coefficient K can be adjusted according to the actual use, and can be set to 1 by default. If the contour error follows poorly, the coefficient can be enhanced.

[0081] At step S150, the joint speed compensation value corresponding to each joint is applied to the corresponding joint to correct and compensate the motion of each joint of the multi-joint robot.

[0082] ​​Specifically, each calculated joint speed compensation value is sent to the speed control loop of the corresponding joint; after the joint speed control loop receives the compensation value, it is superimposed with the original speed command, and the superimposed speed command is sent to the joint motor; the joint motor adjusts the rotation speed according to the new speed command to change the joint motion state, and finally makes the actual position of the end approach the preset expected trajectory, completing the correction and compensation of the contour error.

[0083] The present application directly corrects the joint motion deviation through the complete process of error quantization, PI control and individualized compensation, reduces the end contour error from the root cause, and improves the straight line trajectory accuracy. Through dynamic calculation of the compensation distribution coefficient, it is ensured that the compensation amount always matches the contribution of the joint to the error, avoiding compensation failure caused by attitude change, and adapting to the motion characteristics of multi-joint robots.

[0084] Figure 3 For the cross-coupling controller and the contour error estimation block diagram, the overall process of contour error compensation is: according to the joint angle information returned by the motor, the robot inverse solution is obtained to get the actual coordinates of the robot end, the contour error of the end position and the expected end trajectory in the motion of the multi-joint robot is calculated (the contour error is the distance between the actual current position and the actual straight line trajectory), the contour error is controlled by the cross-coupling controller (the cross-coupling controller is a PI controller with contour error as input value, and the expected value is 0), and the output is multiplied according to the distribution coefficient of the cross-coupling controller in each axis (the distribution coefficient is based on the current robot attitude and the contour error direction), then the joints of the robot are compensated and corrected to reduce the contour error of the robot and improve its straight line trajectory performance.

[0085] Figure 7 For the cross-coupling controller block diagram, the PI controller in the cross-coupling controller is used to control the contour error to 0 to obtain the output value of the PI controller. The output is multiplied by the distribution coefficient of each joint α, β respectively, and then multiplied by the compensation gain coefficient K to obtain the final compensation value. The final compensation value of each joint is used as the speed loop compensation value of each joint, and the compensation is made into the speed loop to correct the final end contour error and improve the accuracy of the end trajectory of the robot.

[0086] According to the technical scheme of the embodiment, the contour error estimation value of the end is determined according to the joint angle information at each joint of the multi-joint robot, the contour error estimation value is input into a preset PI controller, the contour error estimation value is controlled by the PI controller, a PI control output value is obtained, the PI control output value is multiplied by the compensation distribution coefficient corresponding to each joint respectively, the joint speed compensation value corresponding to each joint is obtained, and the joint speed compensation value corresponding to each joint is applied to the corresponding joint, so that the motion of each joint of the multi-joint robot is corrected and compensated. Therefore, the contour error of the multi-joint robot is effectively reduced, and the end trajectory precision and motion stability are improved.

[0087] According to the embodiment of the application, a contour error compensation device of a robot corresponding to the contour error compensation method of the robot is also provided. Referring to Figure 2 The contour error compensation device of the robot can include an angle acquisition unit 101, an error calculation unit 102, a PI control unit 103, a compensation distribution unit 104, and a compensation execution unit 105.

[0088] The angle acquisition unit 101 is configured to acquire joint angle information at each joint of the multi-joint robot.

[0089] The joint angle information refers to real-time data of the current rotation angle of each joint of the multi-joint robot during the motion process. The position of the end of the multi-joint robot is uniquely determined by the joint angles. Only by acquiring the real-time angle of each joint, the actual position of the end can be back calculated. If the angle information of any joint is missing, the actual position of the end cannot be accurately calculated, and it is impossible to determine whether the trajectory deviates from the preset path.

[0090] Each joint of the multi-joint robot is configured with an angle detection device (such as an optical encoder), and the angle detection device operates synchronously with the joint motor to collect real-time joint rotation angle data.

[0091] The error calculation unit 102 is configured to determine a contour error estimation value of the end of the multi-joint robot according to the joint angle information.

[0092] The contour error is a core problem of the straight line trajectory motion of the multi-joint robot. Only by quantifying the error, can a compensation strategy be designed.

[0093] In some embodiments, the error calculation unit 102 determines the contour error estimation value of the end of the multi-joint robot according to the joint angle information, including:

[0094] The error calculation unit 102 is specifically configured to perform inverse solution calculation on the joint angle information of each joint to obtain the actual position coordinates of the multi-joint robot end.

[0095] The joint angle itself cannot directly reflect whether the end deviates from the preset trajectory. Only when the abstract joint angle is converted into intuitive end position coordinates, can the end position coordinates be compared with the coordinates on the expected trajectory to determine whether there is deviation.

[0096] Inverse solution calculation is a calculation process of deducing the actual position coordinates of the robot end effector in the spatial coordinate system based on the kinematics model of the multi-joint robot, through the known joint angle information. The actual position coordinates of the end are the specific coordinate values of the end effector in the preset spatial coordinate system at the current motion moment, reflecting the real space position of the end.

[0097] Specifically, first, the kinematics inverse solution model of the robot is established based on the mechanical structure parameters of the multi-joint robot; during the motion of the robot, the real-time angle information of each joint is collected first, and then the angle information of all joints is substituted into the pre-established kinematics inverse solution model to obtain the X-axis coordinate component and the Y-axis coordinate component of the end effector in the preset spatial coordinate system through operation, and finally the complete actual position coordinates of the end are formed.

[0098] The error calculation unit 102 is specifically configured to obtain the expected position coordinates of the multi-joint robot end on the preset motion trajectory.

[0099] The error calculation unit 102 is specifically configured to calculate the contour error estimation value based on the deviation relationship between the actual position coordinates and the expected position coordinates.

[0100] The expected position coordinates are the ideal coordinate values that the end should reach on the preset motion trajectory corresponding to the current motion moment, which are extracted from the parameter library of the preset motion trajectory.

[0101] When the robot performs linear trajectory operation, due to the existence of following deviation of each joint motor, the final end position will deviate under the joint action. As shown in Figure 4 , the solid line is the preset motion trajectory, the dashed line is the actual motion trajectory, V1 and V2 are unit vectors in the tangential direction of the rotation of two joints respectively, the actual position of the motion end is P point, and the expected position is R point. The coordinates corresponding to P point and R point are as shown in Figure 5 .

[0102] The deviation relationship between the actual position coordinates and the expected position coordinates includes two forms of axial difference and geometric distance, wherein the axial difference can only reflect the deviation in a single direction and cannot directly reflect the overall deviation of the end to the preset trajectory, and the contour error is defined as the shortest distance from the actual position of the end to the preset trajectory, so the geometric distance must be derived based on the deviation relationship to accurately quantify the deviation of the end trajectory.

[0103] In some embodiments, the error calculation unit 102 calculates the contour error estimate value based on the deviation relationship between the actual position coordinates and the expected position coordinates, including:

[0104] The error calculation unit 102 is specifically configured to calculate a first deviation value in the X direction and a second deviation value in the Y direction of the actual position coordinates and the expected position coordinates, respectively.

[0105] The spatial position of the end of the multi-joint robot can be completely described by the X-axis and Y-axis components of the rectangular coordinate system, and the deviation of the end to the preset straight line trajectory is essentially the comprehensive result of the actual position deviating from the expected position in the X and Y directions. Therefore, the complexity of subsequent geometric derivation can be simplified by first calculating the deviation values in the X and Y directions. If the error needs to be calculated in the Z direction, the deviation value of the actual position coordinates and the expected position coordinates in the Z direction also needs to be calculated.

[0106] The first deviation value refers to the numerical difference between the actual position coordinates of the end of the multi-joint robot and the expected position coordinates on the preset motion trajectory in the X-axis direction, and the first deviation value refers to the numerical difference between the actual position coordinates of the end of the multi-joint robot and the expected position coordinates on the preset motion trajectory in the Y-axis direction.

[0107] The calculation formula of the first deviation value E x is:

[0108] E x =|X R -X P |;

[0109] The calculation formula of the second deviation value E Y is:

[0110] E Y =|Y R -Y P |;

[0111] wherein, X R is the X-axis component of the expected position coordinates, X P is the X-axis component of the actual position coordinates, Y R is the Y-axis component of the expected position coordinates, and Y P is the Y-axis component of the actual position coordinates.

[0112] In the motion control of a multi-joint robot, the deviation of the end in the X direction and the Y direction is the basic factor leading to contour error. By calculating the absolute value of the deviation value in the two directions, the deviation degree of the end in each axis direction can be accurately and intuitively obtained. The absolute value is calculated because the deviation value only needs to reflect the size of the deviation, and does not need to pay attention to the specific deviation direction, so that the complexity of subsequent contour error calculation can be simplified, and at the same time, the deviation value is ensured to be non-negative, which meets the basic requirement of quantitatively evaluating the deviation degree. If the absolute value is not used, the deviation value may be positive or negative, and in the subsequent calculation, the contour error calculation result may be distorted due to the sign problem, and the actual deviation degree cannot be accurately reflected.

[0113] The error calculation unit 102 is specifically configured to derive the contour error estimation value through geometric relationship according to the first deviation value, the second deviation value, and the straight line parameter of the preset motion trajectory.

[0114] As shown in Figure 5 When the direction angle of the preset straight line trajectory is θ, the shortest distance (contour error E c ) from the actual position of the end to the straight line can be directly derived through the X direction deviation E x , the Y direction deviation E y and the trigonometric function of the angle θ. Then the calculation formula of the contour error estimation value is:

[0115] E c =-E x sinθ+E y cosθ;

[0116] Wherein, θ is the included angle between the preset straight line trajectory and the positive direction of the X axis.

[0117] By calculating the X / Y axis direction deviation first and then combining the straight line parameter to calculate the shortest distance, the quantitative distortion problem caused by replacing the contour error with a single axis direction deviation is solved, and the error calculation difficulty in the complex trajectory scene is reduced.

[0118] The PI control unit 103 is configured to input the contour error estimation value into a preset PI controller, and control the contour error estimation value through the PI controller to obtain a PI control output value.

[0119] The contour error estimation value is a deviation amount, which needs to be converted into a correction amount through a controller. The PI controller has the characteristics of proportional control fast response and integral control static error elimination, and can effectively convert the contour error into a stable control signal.

[0120] Specifically, the calculated contour error estimation value is taken as an input signal and input into a preset PI controller; the PI controller performs operation on the input error signal according to a preset proportional coefficient (P) and an integral coefficient (I); a proportional element outputs an instant control amount according to the current error size, an integral element outputs a compensation control amount according to the accumulated amount of the error, and the two are superimposed to form a PI control output value; and the PI controller outputs the value.

[0121] The compensation distribution unit 104 is configured to multiply the PI controller output value by a compensation distribution coefficient corresponding to each joint respectively to obtain a joint speed compensation value corresponding to each joint.

[0122] Different joints of the multi-joint robot have different contributions to the end contour error (for example, joints close to the end have a greater impact on the end position, and joints far from the end have a smaller impact), and if the PI control output value is evenly distributed to all joints, some joints may be overcompensated (causing new errors), and some joints may be undercompensated (unable to eliminate the original error). The compensation distribution coefficient is used to distribute the compensation amount according to the joint contribution, so as to ensure that the compensation value of each joint matches its impact on the error.

[0123] In some embodiments, the compensation distribution coefficient corresponding to each joint is calculated based on the angle relationship between the unit vector of the tangent direction of the rotation of each joint of the multi-joint robot in the current posture and the direction of the contour error estimation value, and the angle relationship between the unit vectors of the tangent directions of the rotations of adjacent joints.

[0124] Each joint of the multi-joint robot has different correction capabilities for the end contour error: if the joint rotation direction is consistent with the contour error direction, a slight movement of the joint can significantly correct the error; if the joint rotation direction is perpendicular to the contour error direction, the movement of the joint has little contribution to the correction of the error. Meanwhile, the movement directions of adjacent joints may be coupled, and the compensation distribution needs to be adjusted through the angle relationship between adjacent joints to avoid mutual cancellation of the compensation amount. If the angle relationship is not considered and a fixed coefficient is used to distribute the compensation amount, some joints may be undercompensated or overcompensated.

[0125] The current posture of the robot refers to the overall spatial form of the multi-joint robot formed by the specific angle combination of each joint during movement, which determines the position of the end effector and the influence weight of each joint on the end movement, and dynamically changes with the change of the joint angle. The unit vector of the tangent direction of the joint rotation refers to a unit length vector perpendicular to the joint rotation axis and along the tangent direction of the joint rotation (for example, the unit vector of the tangent direction of the rotation of the joint in the current posture of the robot). Figure 6V1, V2 in FIG. 1), used to describe the direction of the link movement when the joint rotates, which is determined by the joint rotation direction and the robot structure parameters. The contour error estimation value direction points to the shortest distance from the actual position of the multi-joint robot end to the preset motion trajectory, that is, the direction perpendicular to the preset trajectory and pointing to the trajectory, which reflects the target direction that needs to be corrected by the end. The angle between the unit vector of the joint rotation tangent direction and the contour error estimation value direction is Figure 6 θ in FIG. 1 v1 , θ v2 in FIG. 1. The angle between the unit vectors of the tangent directions of adjacent joint rotations is Figure 6 the angle between V1 and V2 in FIG. 1.

[0126] Specifically, first, the current posture of the multi-joint robot is determined: by collecting real-time angle information of each joint, the rotation state of each joint and the overall structure form are determined; second, the unit vectors of the tangent directions of each joint rotation are calculated: according to the mechanical structure parameters of the robot, combined with the current joint angle, the unit vectors of the tangent directions of each joint rotation are derived; then, the contour error estimation value direction is determined, and the perpendicular direction vector from the actual position of the end to the preset trajectory is determined based on the contour error, and the perpendicular direction vector is standardized to a unit vector; next, two types of angles are calculated: one is the angle between the tangent unit vector of each joint and the contour error direction unit vector, and the other is the angle between the tangent unit vectors of adjacent joints; finally, the angles are substituted into the preset coefficient calculation formula to obtain the compensation distribution coefficient corresponding to each joint, and the dynamic calculation of the coefficient is completed.

[0127] The compensation distribution coefficient is calculated through the angle relationship, so that the joint speed compensation value is directly linked to the error correction ability of the joint. The joint with large contribution to error correction is allocated more compensation, and the joint with small contribution is allocated less compensation, avoiding the waste of resources or insufficient correction caused by average allocation, and significantly improving the correction efficiency of the contour error, so that the end trajectory converges to the preset path faster.

[0128] In some embodiments, when the multi-joint robot is a two-joint robot, the calculation formula of the compensation distribution coefficient α corresponding to the first joint is:

[0129]

[0130] The calculation formula of the compensation distribution coefficient β corresponding to the second joint is:

[0131]

[0132] wherein E c is the contour error estimation value, θ1 is the angle between the unit vector of the tangent direction of the first joint rotation and the contour error estimation value direction (such as Figure 6The angle θ between V1 and Ec v1 ), and θ2 is the angle between the unit vector of the tangent direction of the second joint rotation and the direction of the contour error estimation value (as shown in Figure 6 The angle θ between V2 and Ec v2 ), and Φ is the angle between the unit vectors of the tangent directions of the first joint and the second joint rotation.

[0133] According to the structural characteristics of the two-joint robot, the coupling relationship between the joints is quantified by introducing the Φ parameter, and the problem that the compensation effect is weakened due to the neglect of coupling is solved. For example, when the motion directions of the two joints are opposite (Φ is close to 180°), the formula can automatically adjust the proportion of α and β to avoid mutual offset of the compensation amount, so that the compensation of the two joints can work together, and the correction efficiency of the end contour error is significantly improved.

[0134] In some embodiments, after the PI controller output value is multiplied by the compensation distribution coefficient corresponding to each joint respectively, and then multiplied by the compensation gain coefficient K, the result obtained is the compensation value of each joint. The compensation gain coefficient K can be adjusted according to the actual use, and can be defaulted to 1. If the contour error follows poorly, the coefficient can be enhanced.

[0135] The compensation execution unit 105 is configured to apply the joint speed compensation value corresponding to each joint to the corresponding joint respectively to correct and compensate the motion of each joint of the multi-joint robot.

[0136] Specifically, each calculated joint speed compensation value is sent to the speed control loop of the corresponding joint; after the joint speed control loop receives the compensation value, the compensation value is superimposed with the original speed instruction, and the superimposed speed instruction is sent to the joint motor; the joint motor adjusts the rotation speed according to the new speed instruction, drives the joint motion state to change, and finally makes the actual position of the end approach the preset expected trajectory, and completes the correction and compensation of the contour error.

[0137] Through the complete process of error quantization, PI control and individualized compensation, the present application directly corrects the joint motion deviation, reduces the end contour error from the root cause, and improves the straight line trajectory accuracy. Through dynamic calculation of the compensation distribution coefficient, it is ensured that the compensation amount always matches the contribution degree of the joint error, avoiding compensation failure caused by attitude change, and adapting to the motion characteristics of the multi-joint robot.

[0138] Since the processing and functions realized by the device of the present embodiment basically correspond to the embodiments, principles and examples of the foregoing method, details not described in the description of the present embodiment can be referred to the related description in the foregoing embodiments, which will not be repeated here.

[0139] According to the technical scheme of the present application, the contour error estimation value of the end is determined according to the joint angle information at each joint of the multi-joint robot, the contour error estimation value is input into a preset PI controller, the contour error estimation value is controlled by the PI controller to obtain a PI control output value, the PI control output value is multiplied by the compensation distribution coefficient corresponding to each joint respectively to obtain the joint speed compensation value corresponding to each joint, and the joint speed compensation value corresponding to each joint is applied to the corresponding joint to correct and compensate the movement of each joint of the multi-joint robot. Thus, the contour error of the multi-joint robot is effectively reduced, and the end trajectory precision and movement stability are improved.

[0140] According to the embodiment of the present application, a robot system corresponding to the contour error compensation device of the robot is also provided. The robot system can include the contour error compensation device of the robot described above.

[0141] Since the processing and functions realized by the robot system of the present embodiment are basically corresponding to the embodiments, principles and examples of the foregoing device, the descriptions of the present embodiment that are not elaborated can be referred to the related descriptions in the foregoing embodiments, which will not be repeated here.

[0142] According to the technical scheme of the present application, the contour error estimation value of the end is determined according to the joint angle information at each joint of the multi-joint robot, the contour error estimation value is input into a preset PI controller, the contour error estimation value is controlled by the PI controller to obtain a PI control output value, the PI control output value is multiplied by the compensation distribution coefficient corresponding to each joint respectively to obtain the joint speed compensation value corresponding to each joint, and the joint speed compensation value corresponding to each joint is applied to the corresponding joint to correct and compensate the movement of each joint of the multi-joint robot. Thus, the contour error of the multi-joint robot is effectively reduced, and the end trajectory precision and movement stability are improved.

[0143] According to the embodiment of the present application, a storage medium corresponding to the contour error compensation method of the robot is also provided. The storage medium includes a stored program, wherein when the program runs, the device where the storage medium is located is controlled to execute the contour error compensation method of the robot described above.

[0144] Since the processing and functions realized by the storage medium of the present embodiment are basically corresponding to the embodiments, principles and examples of the foregoing method, the descriptions of the present embodiment that are not elaborated can be referred to the related descriptions in the foregoing embodiments, which will not be repeated here.

[0145] According to the technical scheme of the present application, the contour error estimation value of the end is determined according to the joint angle information at each joint of the multi-joint robot, the contour error estimation value is input into a preset PI controller, the contour error estimation value is controlled by the PI controller to obtain a PI control output value, the PI control output value is multiplied by the compensation distribution coefficient corresponding to each joint respectively to obtain the joint speed compensation value corresponding to each joint, and the joint speed compensation value corresponding to each joint is applied to the corresponding joint to correct and compensate the movement of each joint of the multi-joint robot. Thus, the contour error of the multi-joint robot is effectively reduced, and the end trajectory precision and movement stability are improved.

[0146] According to the embodiment of the present application, a computer program product corresponding to the contour error compensation method of the robot is also provided, the computer program product comprises a computer program, and the computer program product is processed to implement the steps of the contour error compensation method of the robot.

[0147] Since the processing and functions implemented by the computer program product of the present embodiment are basically corresponding to the embodiments, principles and examples of the foregoing method, the description of the present embodiment is not detailed, and the relevant description in the foregoing embodiments can be referred to, which is not repeated here.

[0148] According to the technical scheme of the present application, the contour error estimation value of the end is determined according to the joint angle information at each joint of the multi-joint robot, the contour error estimation value is input into a preset PI controller, the contour error estimation value is controlled by the PI controller to obtain a PI control output value, the PI control output value is multiplied by the compensation distribution coefficient corresponding to each joint respectively to obtain the joint speed compensation value corresponding to each joint, and the joint speed compensation value corresponding to each joint is applied to the corresponding joint to correct and compensate the movement of each joint of the multi-joint robot. Thus, the contour error of the multi-joint robot is effectively reduced, and the end trajectory precision and movement stability are improved.

[0149] In summary, those skilled in the art can easily understand that the above-mentioned advantageous modes can be freely combined and superimposed without conflict.

[0150] The above only describes the embodiments of the present application and is not used to limit the present application. Those skilled in the art can make various modifications and changes to the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the scope of the claims of the present application.

Claims

1. A method for contour error compensation of a robot, wherein the robot is a multi-joint robot, characterized in that, The method includes: Obtain the joint angle information at each joint of the multi-joint robot; The contour error estimate of the end effector of the multi-joint robot is determined based on the joint angle information; The estimated contour error value is input into a preset PI controller, and the estimated contour error value is controlled by the PI controller to obtain the PI control output value; The PI controller output value is multiplied by the compensation allocation coefficient corresponding to each joint to obtain the joint speed compensation value corresponding to each joint. The joint velocity compensation value corresponding to each joint is applied to the corresponding joint to correct and compensate the motion of each joint of the multi-joint robot.

2. The robot contour error compensation method according to claim 1, characterized in that, Determining the contour error estimate of the multi-joint robot end effector based on the joint angle information includes: The joint angle information of each joint is inversely calculated to obtain the actual position coordinates of the end effector of the multi-joint robot; Obtain the desired position coordinates of the end effector of the multi-joint robot on the preset motion trajectory; The estimated contour error is calculated based on the deviation between the actual position coordinates and the desired position coordinates.

3. The robot contour error compensation method according to claim 2, characterized in that, Based on the deviation relationship between the actual position coordinates and the expected position coordinates, the estimated value of the contour error is calculated, including: Calculate the first deviation value in the X direction and the second deviation value in the Y direction between the actual position coordinates and the expected position coordinates; The estimated contour error is derived through geometric relationships based on the first deviation value, the second deviation value, and the linear parameters of the preset motion trajectory.

4. The robot contour error compensation method according to claim 3, characterized in that, The first deviation value E x The formula for calculation is: E x =|X R -X P |; The second deviation value E Y The formula for calculation is: AND Y =|Y R -AND P |; Among them, X R X represents the X-axis component of the desired position coordinates. P The X-axis component of the actual position coordinates, Y R Y is the Y-axis component of the desired position coordinates. P The Y-axis component represents the actual position coordinates.

5. The robot contour error compensation method according to claim 1, characterized in that, The compensation allocation coefficient for each joint is calculated based on the angle relationship between the unit vector of the rotation tangent direction of each joint under the current posture of the multi-joint robot and the direction of the estimated contour error, as well as the angle relationship between the unit vectors of the rotation tangent directions of adjacent joints.

6. The robot contour error compensation method according to claim 5, characterized in that, When the multi-joint robot is a two-joint robot, the calculation formula for the compensation allocation coefficient α corresponding to the first joint is: The formula for calculating the compensation distribution coefficient β corresponding to the second joint is: Among them, E c Here, θ1 is the angle between the unit vector of the first joint rotation tangent direction and the direction of the profile error estimate, θ2 is the angle between the unit vector of the second joint rotation tangent direction and the direction of the profile error estimate, and Φ is the angle between the unit vectors of the first joint and the second joint rotation tangent directions.

7. A contour error compensation device for a robot, wherein the robot is a multi-joint robot, characterized in that, The device includes: An angle acquisition unit is configured to acquire joint angle information at each joint of the multi-joint robot. An error calculation unit is configured to determine the estimated contour error of the end effector of the multi-joint robot based on the joint angle information. The PI control unit is configured to input the contour error estimate into a preset PI controller, and control the contour error estimate through the PI controller to obtain a PI control output value; The compensation allocation unit is configured to multiply the output value of the PI controller by the compensation allocation coefficient corresponding to each joint to obtain the joint speed compensation value corresponding to each joint. The compensation execution unit is configured to apply the joint velocity compensation value corresponding to each joint to the corresponding joint to correct and compensate the motion of each joint of the multi-joint robot.

8. A robot system, characterized in that, include: The contour error compensation device for a robot as described in claim 7.

9. A storage medium, characterized in that, The storage medium includes a stored program, wherein, when the program is executed, the device containing the storage medium is controlled to perform the contour error compensation method of the robot according to any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 6.

Images