Encoder fault switching method and system in robot joint control

Abstract

The invention discloses an encoder fault switching method and system in robot joint control, and the method comprises the steps: judging whether the position of an encoder meets the requirements or not in an inductive operation mode, and if yes, using an estimated position as the actual position of the encoder; in the non-inductive operation mode, the estimated rotor position is used as the actual position of the encoder, whether the encoder is allowed to be switched back to the inductive operation mode or not is judged, if yes, whether the position calculated by the encoder is consistent with the estimated rotor position or not is compared, and if yes, whether the normal count of the encoder is larger than the maximum value of the normal count of the encoder or not is judged; if yes, switching back to the inductive operation mode and waiting for processing of the next communication cycle, and if not, directly waiting for processing of the next communication cycle; if the position calculated by the encoder is inconsistent with the estimated position of the rotor, waiting for processing of the next communication cycle; the method has the advantages that the fault can be quickly and accurately detected, and the system can still run stably when the encoder fails.

Description

Technical Field

[0001] The present invention relates to the technical field of robot joint motion control, and in particular to a method and system for switching encoder faults in robot joint control. Background Art

[0002] In the existing robot joint control technology, encoder fault detection is not timely and the processing capacity is limited, resulting in insufficient system reliability. For example, some systems use simple threshold judgment or filtering processing to detect encoder faults. When there is a large jump in the encoder feedback position, the fault is often not detected quickly and accurately, causing the system to continue to use incorrect position information for control, which in turn causes a sudden change in speed and torque, seriously affecting the control effect. In addition, when the existing technology detects an encoder fault, it usually uses the old encoder value sampled before the fault occurs for subsequent calculations or directly takes simple measures such as shutdown or alarm. It lacks an effective fault handling mechanism and cannot ensure that the system can still operate stably when the encoder fails.

[0003] Chinese patent publication number CN108663080A discloses a method for diagnosing faults of an absolute encoder in a servo drive system, the method comprising: determining whether the number of bytes of received data is correct; performing data verification on the received data; calculating the encoder position value increment and comparing it with the increment threshold to determine whether there is an abnormality; in the case of data error, updating the system electrical angle and motor feedback speed according to the recorded position value increment to ensure the smooth operation of the motor and the system position feedback value will not be wrong; after the data error occurs continuously and the number of times exceeds the threshold, an alarm is issued to the system. This patent application uses the old encoder value sampled before the fault occurs for subsequent calculations, which cannot ensure that the system can still operate stably when the encoder fails. Summary of the invention

[0004] The technical problem to be solved by the present invention is that the encoder fault diagnosis method in the prior art cannot detect the fault quickly and accurately, and cannot ensure that the system can still operate stably when the encoder fails.

[0005] The present invention solves the above technical problems by the following technical means: a method for switching encoder faults in robot joint control, comprising:

[0006] Step A: In the sensory operation mode, determine whether the encoder position meets the requirements. If so, use the estimated position as the actual position of the encoder. After confirming the fault, report the fault, choose to switch to the sensorless operation mode or choose to stop and set the operation mode to the sensory operation mode, and then wait for the next communication cycle processing;

[0007] Step B: In the sensorless operation mode, the estimated rotor position is used as the actual position of the encoder to determine whether it is allowed to switch back to the sensory operation mode. If so, proceed to step C. If not, wait for the next communication cycle to be processed;

[0008] Step C: Compare the position calculated by the encoder with the estimated rotor position. If the position calculated by the encoder is consistent with the estimated rotor position, determine whether the normal count of the encoder is greater than the maximum normal count of the encoder. If so, switch back to the sensed operation mode and wait for the next communication cycle processing. If not, directly wait for the next communication cycle processing; if the position calculated by the encoder is inconsistent with the estimated rotor position, wait for the next communication cycle processing.

[0009] Beneficial effects: The present invention monitors the change of position increment in real time and sets a first preset threshold range to quickly and accurately identify the occurrence of encoder failure. When an abnormality is detected, the estimated value is used to replace the actual position of the encoder to participate in subsequent calculations. In addition, after confirming the fault, the present invention can not only report the error in time, but also choose to shut down or enter the non-sensing operation state according to the instruction, ensuring that the system can still operate stably when the encoder fails, thereby improving the stability and reliability of the robot joint control.

[0010] Furthermore, before step A, the method further includes:

[0011] S1. Determine whether the current operation mode is the sensor operation mode. If so, execute step A; if not, execute step B.

[0012] Furthermore, step A comprises:

[0013] S2, the position θ of the encoder feedback k The position θ of the last encoder feedback k-1 Make a difference and determine whether the difference is outside the first preset threshold range. If so, the encoder is faulty and the process goes to S4. If not, the encoder is normal and the process goes to S3.

[0014] S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing;

[0015] S4, using the estimated position θ ks As the actual position θ, it is used in all calculations of the actual position in the subsequent control process of the robot joint, and the encoder correct count TRU is cleared;

[0016] S5, encoder error count value ERR is automatically increased to accumulate the number of faults;

[0017] S6, determine whether the value of the encoder error count value ERR is greater than the preset encoder error maximum value NE. If ERR>NE is detected multiple times, the encoder error count value ERR is cleared and enters S7; otherwise, the fault is not processed temporarily and enters S13;

[0018] S7, report the encoder data error fault, determine whether to choose to continue running, if you choose to continue running, set the running mode to the non-sensing running mode; otherwise, stop directly and set the running mode to the sensored running mode.

[0019] Furthermore, the upper limit of the first preset threshold range is Δθ max , the lower limit is Δθ min , the first preset threshold range is determined according to the previous moment speed n, the previous moment acceleration a and the encoder communication cycle T. The maximum count of the encoder single turn is CPR, then the threshold lower limit Δθ min The calculation formula is Upper threshold Δθ max The calculation formula is α is an adjustable factor.

[0020] Furthermore, in a dual encoder joint, to detect whether the value fed back by a certain encoder is abnormal, replace S2 to S4 in step A with the following process:

[0021] S2. Judging whether the encoder position meets the requirements refers to judging the position θ of the encoder feedback this time. k Is it outside the second preset threshold range? If yes, the encoder is faulty and the process goes to S4; if no, the encoder is normal and the process goes to S3;

[0022] S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing;

[0023] S4, using the estimated position θ ks 'As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint, and at the same time clears the encoder correct count TRU.

[0024] Furthermore, the relationship between the joint load inertia J and the encoder position f(J,θ kref ), f(J,θ kref ) represents the load inertia J and the reference encoder value θ kref The upper limit of the second preset threshold range is Δθ max ', the lower limit is Δθ min ', Δθ min'=f(J,θ kref )-α', Δθ max '=f(J,θ kref )+α', α' is a factor set to prevent misjudgment caused by sampling error; the estimated position θ ks The calculation formula of ' is θ ks '=f(J,θ kref ).

[0025] Furthermore, step B comprises:

[0026] S8. Estimated rotor position θ ks1 As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint;

[0027] S9, determine whether it is necessary to switch back to the sensored operation mode. If it is set to allow switching back, enter S10; otherwise, the encoder normal count TRU is cleared and enter S13.

[0028] Furthermore, step C comprises:

[0029] S10, compare the position θ of the encoder feedback k and the estimated rotor position θ ks1 Is the error within the acceptable range ε? If it is within the acceptable range, that is, |θ k -θ ks1 |<ε, the encoder normal count value TRU increases automatically and enters S11; otherwise, the encoder normal count TRU is reset to zero and enters S13;

[0030] S11, compare the value of the encoder normal count value TRU with the preset encoder normal count maximum value NT, if it has been detected for multiple consecutive times that the encoder normal count TRU is greater than the encoder normal count maximum value NT, then enter S12; otherwise, enter S13;

[0031] S12, the encoder count TRU is reset normally, and the operation mode is set to the sensor operation mode;

[0032] S13: The encoder fault detection and processing process ends and waits for the next cycle processing.

[0033] The present invention also provides an encoder fault switching system in robot joint control, comprising:

[0034] The fault judgment module is used to judge whether the encoder position meets the requirements in the sensory operation mode. If so, the estimated position is used as the actual position of the encoder. After confirming the fault, the fault is reported, and the user can choose to switch to the non-sensing operation mode or stop the machine and set the operation mode to the sensory operation mode, and then wait for the next communication cycle to be processed.

[0035] The mode switching module is used in the sensorless operation mode. It uses the estimated rotor position as the actual position of the encoder to determine whether it is allowed to switch back to the sensory operation mode. If so, the fault handling module is executed. If not, it waits for the next communication cycle to process.

[0036] The fault handling module is used to compare whether the position calculated by the encoder is consistent with the estimated rotor position. If the position calculated by the encoder is consistent with the estimated rotor position, it is determined whether the normal count of the encoder is greater than the maximum normal count of the encoder. If so, it switches back to the sensed operation mode and waits for the next communication cycle processing. If not, it directly waits for the next communication cycle processing; if the position calculated by the encoder is inconsistent with the estimated rotor position, it waits for the next communication cycle processing.

[0037] Furthermore, the fault judgment module also executes:

[0038] S1. Determine whether the current operation mode is the sensor operation mode. If so, execute the fault judgment module. If not, execute the mode switching module.

[0039] Furthermore, the fault judgment module is also used to:

[0040] S2, the position θ of the encoder feedback k The position θ of the last encoder feedback k-1 Make a difference and determine whether the difference is outside the first preset threshold range. If so, the encoder is faulty and the process goes to S4. If not, the encoder is normal and the process goes to S3.

[0041] S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing;

[0042] S4, using the estimated position θ ks As the actual position θ, it is used in all calculations of the actual position in the subsequent control process of the robot joint, and the encoder correct count TRU is cleared;

[0043] S5, encoder error count value ERR is automatically increased to accumulate the number of faults;

[0044] S6, determine whether the value of the encoder error count value ERR is greater than the preset encoder error maximum value NE. If ERR>NE is detected multiple times, the encoder error count value ERR is cleared and enters S7; otherwise, the fault is not processed temporarily and enters S13;

[0045] S7, report the encoder data error fault, determine whether to choose to continue running, if you choose to continue running, set the running mode to the non-sensing running mode; otherwise, stop directly and set the running mode to the sensored running mode.

[0046] Furthermore, the upper limit of the first preset threshold range is Δθ max , the lower limit is Δθ min , the first preset threshold range is determined according to the previous moment speed n, the previous moment acceleration a and the encoder communication cycle T. The maximum count of the encoder single turn is CPR, then the threshold lower limit Δθ min The calculation formula is Upper threshold Δθ max The calculation formula is α is an adjustable factor.

[0047] Furthermore, in a dual encoder joint, to detect whether the value fed back by a certain encoder is abnormal, replace S2 to S4 in step A with the following process:

[0048] S2. Judging whether the encoder position meets the requirements refers to judging the position θ of the encoder feedback this time. k Is it outside the second preset threshold range? If yes, the encoder is faulty and the process goes to S4; if no, the encoder is normal and the process goes to S3;

[0049] S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing;

[0050] S4, using the estimated position θ ks 'As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint, and at the same time clears the encoder correct count TRU.

[0051] Furthermore, the relationship between the joint load inertia J and the encoder position f(J,θ kref ), f(J,θ kref ) represents the load inertia J and the reference encoder value θ kref The upper limit of the second preset threshold range is Δθ max ', the lower limit is Δθ min ', Δθ min '=f(J,θ kref )-α', Δθ max '=f(J,θ kref )+α', α' is a factor set to prevent misjudgment caused by sampling error; the estimated position θ ksThe calculation formula of ' is θ ks '=f(J,θ kref ).

[0052] Furthermore, the mode switching module is also used to:

[0053] S8. Estimated rotor position θ ks1 As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint;

[0054] S9, determine whether it is necessary to switch back to the sensored operation mode. If it is set to allow switching back, enter S10; otherwise, the encoder normal count TRU is cleared and enter S13.

[0055] Furthermore, the fault handling module is also used to:

[0056] S10, compare the position θ of the encoder feedback k and the estimated rotor position θ ks1 Is the error within the acceptable range ε? If it is within the acceptable range, that is, |θ k -θ ks1 |<ε, the encoder normal count value TRU increases automatically and enters S11; otherwise, the encoder normal count TRU is reset to zero and enters S13;

[0057] S11, compare the value of the encoder normal count value TRU with the preset encoder normal count maximum value NT, if it has been detected for multiple consecutive times that the encoder normal count TRU is greater than the encoder normal count maximum value NT, then enter S12; otherwise, enter S13;

[0058] S12, the encoder count TRU is reset normally, and the operation mode is set to the sensor operation mode;

[0059] S13: The encoder fault detection and processing process ends and waits for the next cycle processing.

[0060] The advantages of the present invention are:

[0061] (1) The present invention monitors the change of position increment in real time and sets a preset threshold range to quickly and accurately identify the occurrence of encoder failure. When an abnormality is detected, the estimated value is used to replace the actual position of the encoder to participate in subsequent calculations. In addition, after confirming the fault, the present invention can not only report the error in time, but also choose to shut down or enter the non-sensing operation state according to the instruction, ensuring that the system can still operate stably when the encoder fails, thereby improving the stability and reliability of the robot joint control.

[0062] (2) After confirming that the encoder has failed, the present invention promptly reports the fault and selects to shut down or switch to a non-sensing running state according to the command, thus flexibly adapting to different safety requirements. After multiple consecutive detections of normality, the present invention can provide a dual selection of sensed / non-sensing continued running mode according to the command requirements, thus avoiding frequent switching of control modes and ensuring reliable and efficient operation of the robot joints after the fault is eliminated.

[0063] (3) The encoding anomaly detection and fault tolerance process of the present invention is simple and fast: by comparing the predicted position increment with the encoder feedback increment, the encoder anomaly is detected in real time. The method is simple and fast. Using the predicted position value as the position replacement value at the time of a fault is more accurate than using the correct value at the previous moment. It can also filter out the interference of occasional encoder abnormal values ​​and can better deal with the situation where fault values ​​appear continuously.

[0064] (4) The state switching mechanism of the present invention is flexible: when the encoder abnormality is detected multiple times, it can choose to shut down or switch to the non-sensing operation state according to the instructions, providing a flexible emergency handling plan. After detecting that the encoder has returned to normal, it can switch back to the normal sensed operation state as required, which can effectively avoid frequent switching and ensure the reliable operation of the robot joints. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 A flowchart of a method for switching encoder faults in robot joint control disclosed in an embodiment of the present invention;

[0066] Figure 2 A schematic diagram of comparing the joint torque and speed before and after the encoder appears abnormal and before and after fault-tolerant processing in a robot joint control encoder fault switching method disclosed in an embodiment of the present invention, wherein: Figure 2 (a) is a schematic diagram of the torque without fault. Figure 2 (b) is a schematic diagram of the unprocessed torque. Figure 2 (c) is the torque diagram after fault handling. Figure 2 (d) is a schematic diagram of the speed without fault. Figure 2 (e) is a schematic diagram of the unprocessed speed. Figure 2 (f) is a schematic diagram of the speed after the fault is resolved. DETAILED DESCRIPTION

[0067] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0068] Example 1

[0069] like Figure 1 As shown, Embodiment 1 of the present invention provides a method for switching encoder faults in robot joint control, comprising the following steps:

[0070] S1. Determine the current operation mode. If it is the sensory operation mode, proceed to step S2; otherwise, proceed to step S8.

[0071] The sensed operation mode and the non-sensing operation mode are two different operation modes of robots in the prior art. The sensed operation refers to operation with position sensors. Position sensors are integrated in the robot joints, and the positions of the joints are detected in real time by sensors, thereby achieving precise control of the joints. This mode of operation is suitable for scenarios that require high positioning accuracy and high dynamic response. Non-sensing operation does not rely on position sensors, and estimates the position of joints through algorithms. It is suitable for cost-sensitive applications and does not require low-speed high-precision control. The main features of non-sensing operation are that it does not require additional sensors, low cost, and simple structure, but its algorithm is highly complex, and the control accuracy and stability may not be as good as sensed operation.

[0072] S2. Calculate the position θ of the encoder feedback based on the value of the encoder feedback. k and the position θ of the last encoder feedback k-1 Difference θ k –θ k-1 , determine the difference θ k –θ k-1 Is it within the preset threshold [Δθ min ,Δθ max ]. If the difference exceeds the preset threshold, that is, |θ k –θ k-1 |<Δθ min Or |θ k –θ k-1 |>Δθ max , trigger fault judgment and go to step S4, otherwise go to step S3.

[0073] Note that the preset threshold here is not limited to a certain setting method. The example method is as follows: the preset threshold is determined based on the previous moment speed n, the previous moment acceleration a and the encoder communication cycle T. The speed unit is RPM, the acceleration unit is RPM / s, the encoder communication cycle T unit is s, and the maximum count of the encoder single turn is CPR. The calculation formula for the preset threshold is: α is a factor set to prevent misjudgment caused by sampling error, and can be adjusted according to actual needs.

[0074] S3, the encoder has no abnormality, and the position θ is fed back by the encoder. kThe actual position θ is used as the actual position to participate in all calculations that require the actual position in the subsequent control process, and then enters step S13 to wait for the next communication cycle processing.

[0075] S4, encoder abnormality occurs, use the estimated position θ ks As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process, and at the same time clears the encoder correct count TRU.

[0076] Note that the estimated position calculation here is not limited to a certain method. The example method is as follows: Based on the position θ fed back by the last encoder k-1 , the speed n at the last moment, the acceleration a at the last moment and the encoder communication cycle T are used to calculate the estimated position. The speed unit is RPM, the acceleration unit is RPM / s, the encoder communication cycle T unit is s, and the maximum count of the encoder single turn is CPR. The calculation formula for the estimated position is:

[0077] The process from step S2 to step S4 can use a variety of schemes to determine the occurrence of abnormalities and calculate the position replacement value when the fault occurs for different joints or different control systems. For example, in a dual-encoder joint, to detect whether the value fed back by a certain encoder is abnormal, step S2 can be replaced by calculating and setting the preset threshold and estimated position through the joint load inertia and the value of another encoder; the estimated position is used to participate in all calculations that require the actual position in the subsequent control process. The details are as follows:

[0078] S2. Calculate the position θ of the encoder feedback based on the value of the encoder feedback. k , determine θ k Is it within the preset threshold [Δθ min ',Δθ max ']. The specific method is: according to the physical characteristics and kinematic model of the joint, determine the relationship between the joint load inertia and the encoder position, that is, fit a function f through the experimental data so that f(J,θ kref ) represents the load inertia J and the reference encoder value θ kref The expected position under load inertia J can be defaulted, and the preset threshold is set to [Δθ min ',Δθ max '], where Δθ min '=f(J,θ kref )-α', Δθ max '=f(J,θ kref )+α' is a factor set to prevent misjudgment caused by sampling error, which can be adjusted according to actual needs. k exceeds the preset threshold, i.e., θ k <Δθ min 'or θk >Δθ max ', trigger fault judgment and go to step S4, otherwise go to step S3.

[0079] S3, the encoder has no abnormality, and the position θ is fed back by the encoder. k The actual position θ is used as the actual position to participate in all calculations that require the actual position in the subsequent control process, and then enters step S13 to wait for the next communication cycle processing.

[0080] S4, encoder abnormality occurs, use the estimated position θ ks 'As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process. The calculation formula for the estimated position is θ ks '=f(J,θ kref ), and at the same time, the encoder correct count TRU is cleared. This part is a replacement solution. Figure 1 Not shown.

[0081] S5. The encoder error count value ERR increases automatically to accumulate the number of faults.

[0082] S6. By comparing the encoder error count value ERR with the preset encoder error maximum value NE, decide whether to report the fault. If multiple transition faults are detected, that is, ERR>NE, the encoder error count value ERR is cleared and the process goes to step S7; otherwise, the fault is not processed for the time being and the process goes to step S13 to wait for the next communication cycle to process.

[0083] S7, report the encoder data error fault, and determine whether to continue running according to the preset strategy, that is, the post-fault control flag FAULT_STOP. If FAULT_STOP is 0, choose to continue running, and set the running mode to non-sensing operation; otherwise, stop directly and set the running mode to sensored operation. The "strategy" of the preset strategy refers to the two options of "continue running after fault" and "stop after fault". The "preset" is the strategy that the user sets before running to execute after the fault. For example, set the post-fault control flag FAULT_STOP in the algorithm. Before the joint runs, write 1 to the flag FAULT_STOP, which means to stop directly after the fault, and write 0 to the flag FAULT_STOP, which means to continue running after the fault.

[0084] S8, after determining in step S1 that the current operation mode is the sensorless operation mode, estimate the rotor position θ by using a suitable sensorless operation strategy ks1As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process. The sensorless operation strategy can be any strategy in the prior art that can estimate the rotor position. For example, the document "Wang Yaoqiang, Feng Yutao, Qin Ming, et al. Full-order sliding mode observation and control strategy for surface-mounted permanent magnet synchronous motor [J]. Transactions of China Electrotechnical Society, 2018, 33(24): 5688-5699. DOI: 10.19595 / j.cnki.1000-6753.tces.181133." describes a sensorless sliding mode control strategy for surface-mounted permanent magnet synchronous motor based on a full-order sliding mode observer structure (FSMO). The full-order sliding mode observer and phase-locked loop are used to replace the traditional position sensor to estimate the rotor position.

[0085] S9. Determine whether it is necessary to switch back to the sensed operation mode according to the flag CTRL_SWITCH that allows switching back to the sensed operation mode. If it is set to allow switching back, that is, CTRL_SWITCH is 1, then go to step S10; otherwise, the encoder normal count TRU is cleared, and go to step S13. This step can be set in the algorithm to allow switching back to the sensed operation flag CTRL_SWITCH. Before the joint runs, write 1 to the flag CTRL_SWITCH, which means that after the encoder is detected to be normal in the non-sensor operation mode, it is possible to prepare to switch back to the sensed operation mode. Write 0 to the flag CTRL_SWITCH, which means that it is not allowed to switch back to the sensed operation mode in the non-sensor operation mode.

[0086] S10, compare the position θ calculated by the encoder this time k and estimate θ ks1 Is the error within the acceptable range ε? If it is within the acceptable range, that is, |θ k -θ ks1 |<ε, the encoder normal count value TRU increases automatically, and the process goes to step S11; otherwise, the encoder normal count TRU is reset to zero, and the process goes to step S13.

[0087] S11. Compare the encoder normal count value TRU with the preset encoder normal count maximum value NT. If it has been detected for multiple consecutive times that the encoder normal count TRU is greater than the encoder normal count maximum value NT, proceed to step S12; otherwise, proceed to step S13.

[0088] S12, the encoder normal count TRU is cleared, and the operation mode is set to the sensor operation mode.

[0089] S13: The encoder fault detection and processing process ends and waits for the next cycle processing.

[0090] Figure 2This is the comparison of joint torque and speed before and after the encoder abnormality and fault tolerance processing in the robot joint control. Figure 2 (a) is a schematic diagram of the torque without fault. Figure 2 (b) is a schematic diagram of the unprocessed torque. Figure 2 (c) is the torque diagram after fault handling. Figure 2 (d) is a schematic diagram of the speed without fault. Figure 2 (e) is a schematic diagram of the unprocessed speed. Figure 2 (f) is a schematic diagram of the rotation speed after fault processing. After the encoder detection process proposed by the present invention is completed, the position used to calculate and update the system electrical angle and motor speed is not the position of the encoder feedback or the position recorded when the last data was correct, but the estimated value after calculation and prediction, which can guarantee the continuous and reliable operation of the joint in the case of encoder data errors to the greatest extent possible, while avoiding the system performance degradation caused by frequent fault switching, and even the problem of failure to respond in time in some cases.

[0091] Through the above technical scheme, the present invention uses the predicted position value as the position replacement value when a fault occurs, which can take into account both control continuity and accuracy, and is better than simply using the old value; it provides an intelligent switching mechanism, which can provide a dual selection of shutdown / non-sensing continued operation mode according to instruction requirements after the cumulative number of faults, and flexibly adapt to different safety requirements; after multiple consecutive detections of normality, it can provide a dual selection of sensed / non-sensing continued operation mode according to instruction requirements, which not only avoids frequent switching of control methods, but also ensures the reliable operation of the robot joints after the fault is eliminated; the fault detection and fault-tolerant schemes used are simple and changeable, without the need to add any hardware and too many algorithms, and multiple steps can be flexibly configured.

[0092] Example 2

[0093] The present invention also provides an encoder fault switching system in robot joint control, comprising:

[0094] The fault judgment module is used to judge whether the encoder position meets the requirements in the sensory operation mode. If so, the estimated position is used as the actual position of the encoder. After confirming the fault, the fault is reported, and the user can choose to switch to the non-sensing operation mode or stop the machine and set the operation mode to the sensory operation mode, and then wait for the next communication cycle to be processed.

[0095] The mode switching module is used in the sensorless operation mode. It uses the estimated rotor position as the actual position of the encoder to determine whether it is allowed to switch back to the sensory operation mode. If so, the fault handling module is executed. If not, it waits for the next communication cycle to process.

[0096] The fault handling module is used to compare whether the position calculated by the encoder is consistent with the estimated rotor position. If the position calculated by the encoder is consistent with the estimated rotor position, it is determined whether the normal count of the encoder is greater than the maximum normal count of the encoder. If so, it switches back to the sensed operation mode and waits for the next communication cycle processing. If not, it directly waits for the next communication cycle processing; if the position calculated by the encoder is inconsistent with the estimated rotor position, it waits for the next communication cycle processing.

[0097] Specifically, the fault judgment module also executes:

[0098] S1. Determine whether the current operation mode is the sensor operation mode. If so, execute the fault judgment module. If not, execute the mode switching module.

[0099] More specifically, the fault judgment module is also used to:

[0100] S2, the position θ of the encoder feedback k The position θ of the last encoder feedback k-1 Make a difference and determine whether the difference is outside the first preset threshold range. If so, the encoder is faulty and the process goes to S4. If not, the encoder is normal and the process goes to S3.

[0101] S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing;

[0102] S4, using the estimated position θ ks As the actual position θ, it is used in all calculations of the actual position in the subsequent control process of the robot joint, and the encoder correct count TRU is cleared;

[0103] S5, encoder error count value ERR is automatically increased to accumulate the number of faults;

[0104] S6, determine whether the value of the encoder error count value ERR is greater than the preset encoder error maximum value NE. If ERR>NE is detected multiple times, the encoder error count value ERR is cleared and enters S7; otherwise, the fault is not processed temporarily and enters S13;

[0105] S7, report the encoder data error fault, determine whether to choose to continue running, if you choose to continue running, set the running mode to the non-sensing running mode; otherwise, stop directly and set the running mode to the sensored running mode.

[0106] More specifically, the upper limit of the first preset threshold range in S2 is Δθ max , the lower limit is Δθ min, the first preset threshold range is determined according to the previous moment speed n, the previous moment acceleration a and the encoder communication cycle T. The maximum count of the encoder single turn is CPR, then the threshold lower limit Δθ min The calculation formula is Upper threshold Δθ max The calculation formula is α is an adjustable factor.

[0107] More specifically, in a dual encoder joint, to detect whether the value fed back by a certain encoder is abnormal, replace S2 to S4 in step A with the following process:

[0108] S2. Judging whether the encoder position meets the requirements refers to judging the position θ of the encoder feedback this time. k Is it outside the second preset threshold range? If yes, the encoder is faulty and the process goes to S4; if no, the encoder is normal and the process goes to S3;

[0109] S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing;

[0110] S4, using the estimated position θ ks 'As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint, and at the same time clears the encoder correct count TRU.

[0111] More specifically, the relationship between the joint load inertia J and the encoder position f(J,θ kref ), f(J,θ kref ) represents the load inertia J and the reference encoder value θ kref The upper limit of the second preset threshold range is Δθ max ', the lower limit is Δθ min ', Δθ min '=f(J,θ kref )-α', Δθ max '=f(J,θ kref )+α', α' is a factor set to prevent misjudgment caused by sampling error; the estimated position θ ks The calculation formula of ' is θ ks '=f(J,θ kref ).

[0112] More specifically, the mode switching module is also used to:

[0113] S8. Estimated rotor position θ ks1As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint;

[0114] S9, determine whether it is necessary to switch back to the sensored operation mode. If it is set to allow switching back, enter S10; otherwise, the encoder normal count TRU is cleared and enter S13.

[0115] More specifically, the fault handling module is also used to:

[0116] S10, compare the position θ of the encoder feedback k and the estimated rotor position θ ks1 Is the error within the acceptable range ε? If it is within the acceptable range, that is, |θ k -θ ks1 |<ε, the encoder normal count value TRU increases automatically and enters S11; otherwise, the encoder normal count TRU is reset to zero and enters S13;

[0117] S11, compare the value of the encoder normal count value TRU with the preset encoder normal count maximum value NT, if it has been detected for multiple consecutive times that the encoder normal count TRU is greater than the encoder normal count maximum value NT, then enter S12; otherwise, enter S13;

[0118] S12, the encoder count TRU is reset normally, and the operation mode is set to the sensor operation mode;

[0119] S13: The encoder fault detection and processing process ends and waits for the next cycle processing.

[0120] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for switching encoder faults in robot joint control, characterized in that: include: Step A: In the sensory operation mode, determine whether the encoder position meets the requirements. If so, use the estimated position as the actual position of the encoder. After confirming the fault, report the fault, choose to switch to the sensorless operation mode or choose to stop and set the operation mode to the sensory operation mode, and then wait for the next communication cycle processing; Step B: In the sensorless operation mode, the estimated rotor position is used as the actual position of the encoder to determine whether it is allowed to switch back to the sensory operation mode. If so, proceed to step C. If not, wait for the next communication cycle to be processed; Step C: Compare the position calculated by the encoder with the estimated rotor position. If the position calculated by the encoder is consistent with the estimated rotor position, determine whether the normal count of the encoder is greater than the maximum normal count of the encoder. If so, switch back to the sensed operation mode and wait for the next communication cycle processing. If not, directly wait for the next communication cycle processing; if the position calculated by the encoder is inconsistent with the estimated rotor position, wait for the next communication cycle processing.

2. The encoder fault switching method in robot joint control according to claim 1 is characterized in that: Step A also includes: S1. Determine whether the current operation mode is the sensor operation mode. If so, execute step A; if not, execute step B.

3. The encoder fault switching method in robot joint control according to claim 2 is characterized in that: Step A includes: S2, judging whether the encoder position meets the requirements means that the position θ of the encoder feedback is k The position θ of the last encoder feedback k-1 Make a difference and determine whether the difference is outside the first preset threshold range. If so, the encoder is faulty and the process goes to S4. If not, the encoder is normal and the process goes to S3. S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing; S4, using the estimated position θ ks As the actual position θ, it is used in all calculations of the actual position in the subsequent control process of the robot joint, and the encoder correct count TRU is cleared; S5, encoder error count value ERR is automatically increased to accumulate the number of faults; S6, determine whether the value of the encoder error count value ERR is greater than the preset encoder error maximum value NE. If ERR>NE is detected multiple times, the encoder error count value ERR is cleared and the process goes to S7; otherwise, the fault is not processed temporarily and the process goes to S13; S7, report the encoder data error fault, determine whether to choose to continue running, if you choose to continue running, set the running mode to the non-sensing running mode; otherwise, stop directly and set the running mode to the sensored running mode.

4. A method for switching encoder faults in robot joint control according to claim 3, characterized in that: The upper limit of the first preset threshold range is Δθ max , the lower limit is Δθ min , the first preset threshold range is determined according to the previous moment speed n, the previous moment acceleration a and the encoder communication cycle T. The maximum count of the encoder single turn is CPR, then the threshold lower limit Δθ min The calculation formula is Upper threshold Δθ max The calculation formula is α is an adjustable factor; the estimated position θ ks The calculation formula is 5. The encoder fault switching method in robot joint control according to claim 3 is characterized in that: In a dual-encoder joint, to detect whether the value of a certain encoder feedback is abnormal, replace S2 to S4 in step A with the following process: S2. Judging whether the encoder position meets the requirements refers to judging the position θ of the encoder feedback this time. k Is it outside the second preset threshold range? If yes, the encoder is faulty and the process goes to S4; if no, the encoder is normal and the process goes to S3; S3, using encoder feedback position θ k As the actual position θ, it participates in all calculations of the actual position required in the subsequent control process of the robot joint, and enters S13, which is to wait for the next communication cycle processing; S4, using the estimated position θ ks 'As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint, and at the same time clears the encoder correct count TRU.

6. A method for switching encoder faults in robot joint control according to claim 5, characterized in that: The relationship between the joint load inertia J and the encoder position f(J,θ kref ), f(J,θ kref ) represents the load inertia J and the reference encoder value θ kref The upper limit of the second preset threshold range is Δθ max ', the lower limit is Δθ min ', Δθ min '=f(J,θ kref )-α', Δθ max '=f(J,θ kref )+α', α' is a factor set to prevent misjudgment caused by sampling error; the estimated position θ ks The calculation formula of ' is θ ks '=f(J,θ kref ).

7. The encoder fault switching method in robot joint control according to claim 5, characterized in that: Step B includes: S8. Estimated rotor position θ ks1 As the actual position θ, it participates in all calculations that require the actual position in the subsequent control process of the robot joint; S9, determine whether it is necessary to switch back to the sensored operation mode. If it is set to allow switching back, enter S10; otherwise, the encoder normal count TRU is cleared and enter S13.

8. The encoder fault switching method in robot joint control according to claim 7, characterized in that: Step C includes: S10, compare the position θ of the encoder feedback k and the estimated rotor position θ ks1 Is the error within the acceptable range ε? If it is within the acceptable range, that is, |θ k -θ ks1 |<ε, the encoder normal count value TRU increases automatically and enters S11; otherwise, the encoder normal count TRU is reset to zero and enters S13; S11, compare the value of the encoder normal count value TRU with the preset encoder normal count maximum value NT, if it has been detected for multiple consecutive times that the encoder normal count TRU is greater than the encoder normal count maximum value NT, then enter S12; otherwise, enter S13; S12, the encoder count TRU is reset normally, and the operation mode is set to the sensor operation mode; S13: The encoder fault detection and processing process ends and waits for the next cycle processing.

9. An encoder fault switching system in robot joint control, characterized in that: include: The fault judgment module is used to judge whether the encoder position meets the requirements in the sensory operation mode. If so, the estimated position is used as the actual position of the encoder. After confirming the fault, the fault is reported, and the user can choose to switch to the non-sensing operation mode or stop the machine and set the operation mode to the sensory operation mode, and then wait for the next communication cycle to be processed. The mode switching module is used in the sensorless operation mode. It uses the estimated rotor position as the actual position of the encoder to determine whether it is allowed to switch back to the sensory operation mode. If so, the fault handling module is executed. If not, it waits for the next communication cycle to process. The fault handling module is used to compare whether the position calculated by the encoder is consistent with the estimated rotor position. If the position calculated by the encoder is consistent with the estimated rotor position, it is determined whether the normal count of the encoder is greater than the maximum normal count of the encoder. If so, it switches back to the sensed operation mode and waits for the next communication cycle processing. If not, it directly waits for the next communication cycle processing; if the position calculated by the encoder is inconsistent with the estimated rotor position, it waits for the next communication cycle processing.

10. The encoder fault switching system in robot joint control according to claim 9, characterized in that: The fault judgment module also executes: S1. Determine whether the current operation mode is the sensor operation mode. If so, execute the fault judgment module. If not, execute the mode switching module.

Images