Motion control system for a robot flexible joint module based on dual encoders

Abstract

The application discloses a motion control system of a robot flexible joint module based on double encoders, and the flexible joint module comprises a drive controller, a motor, a first encoder and a second encoder. The drive controller comprises a communication module, a control module and an intelligent drive module which are sequentially connected. The intelligent drive module is electrically connected with the motor. The first encoder and the second encoder are both connected with the control module through the communication module. The intelligent drive module detects three-phase current and bus voltage of the motor and sends them to the control module. Meanwhile, the intelligent drive module receives and amplifies the control signal output by the control module to drive the motor to move. The control module runs a flexible joint motion control algorithm according to the position information of the motor end and the load end, the three-phase current of the motor and the bus voltage, and outputs the control signal to the intelligent drive module. The application has the advantages of small structure size, easy use and high positioning accuracy of the joint.

Description

Technical Field

[0001] This invention relates to the field of robot joint control, and more specifically to a motion control system for a robot flexible joint module based on dual encoders. Background Technology

[0002] With the deep integration of new-generation information technology and manufacturing, the global intelligent manufacturing industry is developing rapidly, and robots are being used more and more widely in the industrial field. Joint modules are the core components of building a robot's motion system, and their performance directly affects the robot's motion stability, accuracy, safety, and lifespan.

[0003] Currently, most robots or robotic arms use rigid joints, employing rigid control for fast response and high precision. However, with the demands of modern industrialization and intelligentization, robots and robotic arms not only need accurate trajectories but also require handling more collision-prone tasks. Introducing elastic elements into the joint is one way to give robot or robotic arm joints compliant properties and enhance impact resistance, as illustrated by Chinese Patent Publication No. CN103817695A, which discloses a control method and drive device for a flexible robot joint. However, with the introduction of elastic elements, due to the flexibility of both the elastic element and the reducer, when the joint end experiences a certain torque and angular deviation, the angular deviation sensed by the motor will be less than the theoretical angle calculated from the reduction ratio, leading to a loss of positioning accuracy at the end. Therefore, relying solely on motor position and speed feedback for closed-loop control results in low joint positioning accuracy and hinders fine motion planning.

[0004] A mainstream approach to high-precision control of robot joint end effectors is to use multi-turn absolute encoders or multiple absolute encoders powered by batteries to ensure accurate joint position information. Multi-turn absolute encoders require battery power; once the battery is installed inside the joint, it cannot be used for extended periods. When the battery runs out, the joint needs to be disassembled, making subsequent maintenance very troublesome. A common dual-encoder solution involves installing two encoders, one at the motor output and the other at the joint output. End-effector rotation deviations caused by joint flexibility can be corrected by the motor pulling back at a certain angle, thus compensating for joint flexibility. However, this dual-encoder solution generally results in a larger robot joint structure, especially in the axial dimension, making it inconvenient to use.

[0005] In summary, solutions to the above problems need to be proposed by starting with the structure and control of the joint module. Summary of the Invention

[0006] The technical problem to be solved by the present invention is that the existing robot joint control methods have problems such as large structural size, inconvenience of use, and low joint positioning accuracy.

[0007] This invention solves the above-mentioned technical problems through the following technical means: a motion control system for a robot flexible joint module based on dual encoders. The flexible joint module includes a drive controller, a motor, a first encoder, and a second encoder. The first and second encoders are absolute position encoders. The drive controller includes a communication module, a control module, and an intelligent drive module connected in sequence. The intelligent drive module is electrically connected to the motor. Both the first and second encoders are connected to the control module through the communication module. The first and second encoders provide the control module with position information of the motor end and the load end. The intelligent drive module detects the three-phase current and bus voltage of the motor and sends them to the control module. At the same time, it receives and amplifies the control signal output by the control module to drive the motor to move. The control module runs a flexible joint motion control algorithm based on the received position information of the motor end and the load end, as well as the three-phase current and bus voltage of the motor, and outputs a control signal to the intelligent drive module.

[0008] Furthermore, the flexible joint module also includes a harmonic reducer, an elastic element, a load-side output shaft, and a joint housing. Both the first and second encoders include a magnetic grating and an encoder chip, both of which are annular. The encoder chip is fitted over the magnetic grating. The magnetic grating of the first encoder is fitted onto and fixedly connected to the motor shaft. The motor shaft is also coaxially and fixedly connected to the input shaft of the harmonic reducer. The output shaft of the harmonic reducer is fixedly connected to the elastic element, and the elastic element is fixedly connected to the magnetic grating of the second encoder. The magnetic grating of the second encoder is also fixedly connected to the load-side output shaft. The joint housing has a hollow structure and is provided at both ends. The joint housing has a through hole, through which the motor shaft passes to connect to an external device. The load-side output shaft passes through the through hole at the other end of the joint housing and acts on an object. The encoder chip of the first encoder, the motor housing, the encoder chip of the second encoder, and the drive controller are all fixed on the joint housing. The intelligent drive module includes a current sensor, a bus voltage detection unit, a three-phase inverter bridge and its gate drive unit. The current sensor is used to detect the three-phase current of the motor, the bus voltage detection unit is used to detect the bus voltage, and the three-phase inverter bridge and its gate drive unit are used to drive the motor.

[0009] Furthermore, the flexible joint motion control algorithm includes the following steps:

[0010] Step 1: Calibrate the zero reference value of the current sensor in the intelligent drive module;

[0011] Step 2: Identify the motor stator resistance and direct-axis inductance, and calculate the current loop control parameters. The current loop control parameters include the proportional parameter k of the PI controller. cp The integral parameter k of the PI controller ci ;

[0012] Step 3: Calibrate the difference pos1 between the zero position of the first encoder and the zero electrical angle of the motor;

[0013] Step 4: Correct the difference pos2 between the zero position of the second encoder and the mechanical angle zero of the joint output terminal. The joint output terminal is the load terminal.

[0014] Step 5: First, start the current closed-loop calculation. Then, according to the host computer instructions, enter the speed loop, motor end position loop, and load end position loop control modes respectively to perform corresponding closed-loop control. During the control process, the calculation result of the load end position loop is sent to the motor end position loop or speed loop, the calculation result of the motor end position loop is sent to the speed loop, the calculation result of the speed loop is sent to the current loop, the current loop calculation result is subjected to Park inverse transformation, and the transformation result is used as the input of space vector modulation, and then SVPWM calculation is performed to obtain the PWM signal, which is sent to the intelligent drive module to drive the motor to move.

[0015] Furthermore, step two includes:

[0016] Step 21: Stator resistance identification;

[0017] A DC voltage U is applied to the motor using a drive controller, with phase A being positive and phases B and C being negative, causing the motor rotor to reach a fixed position where the shaft of phase A coincides with the direct shaft. The phase A current is collected N times, and the average value I of these N currents is calculated. N Through R s =2U / 3I N The stator resistance R is calculated. s ;

[0018] Step 22: Direct-axis inductor identification;

[0019] After identifying the stator resistance, the steady-state current I is calculated. s The drive controller injects pulse voltage into the motor and records the motor's direct-axis current i. d The time t required to reach 0.632 times the steady-state current is calculated according to the formula L. d =tR s The direct-axis inductance value is calculated, and the average value of multiple calculations is used to obtain the direct-axis inductance identification result.

[0020] Step 23: Based on the stator resistance and direct-axis inductance of the motor, calculate the control parameters of the current loop controller in the flexible joint motion control algorithm, and store the control parameters in Flash. The calculation formula for the control parameters is as follows:

[0021]

[0022] Where, k cp k represents the proportional parameter of the PI controller. ciThis represents the integral parameter of the PI controller, and f represents the current loop control frequency.

[0023] Furthermore, step three includes:

[0024] Step 31: Record the value P of the first encoder when the motor electrical angle is zero. 11 The drive controller applies voltage to the motor, causing the motor rotor to rotate to an electrical angle of 2π in steps of π / 3 electrical angles each time. The value P of the first encoder at this point is recorded. 12 According to P 11 With P 12 The difference is used to determine whether the positive direction of motor rotation is consistent with the direction of the increase in the value of the first encoder. If they are consistent, the direction flag 1 is 1; otherwise, the direction flag 1 is -1.

[0025] Step 32: Apply voltage to the motor via the drive controller, causing the motor rotor to rotate to a mechanical angle of 2π in increments of π / 3 electrical angles each time. Record the value P of the first encoder when the rotor reaches each electrical angle of 0. 11 P 12 ...P 1p Where p is the number of pole pairs of the motor; let P be the sum of the p values ​​of the first encoder. sum =P 11 +P 12 +……+P 1p Wherein, electrical angle = mechanical angle × number of pole pairs of the motor;

[0026] Step 33: The maximum single-turn count of the first encoder is CPR, then the difference between its zero position and the zero electrical angle of the motor is pos1.

[0027] Step 34: Store pos1 and direction flag 1 into Flash.

[0028] Furthermore, step four includes:

[0029] Step 41: Record the value P of the second encoder when the motor electrical angle is zero. 21 The drive controller applies voltage to the motor, causing the motor rotor to rotate to an electrical angle of 2π in steps of π / 3 electrical angles each time. The value P of the second encoder at this point is recorded. 22 According to P 21 With P 22 The difference is used to determine whether the positive direction of the motor rotation is consistent with the direction of the increase in the value of the second encoder. If they are consistent, the direction flag bit Flag2 is 1; otherwise, the direction flag bit Flag2 is -1.

[0030] Step 42: After the flexible joint module is assembled into the robot or robotic arm, set the configuration of the robot or robotic arm to a state where the mechanical angle at the end of the flexible joint is zero, and record the value of the second encoder at this time, which is the difference pos2 between the zero position of the second encoder and the zero mechanical angle at the joint output end.

[0031] Step 43: Store pos2 and direction flag 2 into Flash.

[0032] Furthermore, if the host computer command is for the load-side position loop, it determines whether the motor-side position loop needs to be run. If so, it starts the motor-side position loop calculation first, then starts the load-side position loop calculation, calculates and sets the target mechanical position of the motor-side position loop. Otherwise, directly start the load-side position loop calculation, calculate and set the target speed of the speed loop.

[0033] The load-side position closed-loop calculation includes calculating the load-side feedback position of the motor based on the values ​​P2(k) and pos2 of the second encoder and the direction flag 2. The zero-crossing determination is performed based on the difference between the target load end position and the load end feedback position used by the motor for calculation, thus obtaining the load end feedback position θ used for calculation. m2 (k), and then the target rotational speed of the rotational speed loop at time k is calculated. Or the target mechanical position around the motor end position The formula is as follows:

[0034]

[0035] Where, k pp k represents the proportional parameter of the load-side position loop PI controller. pi e represents the integral parameter of the load-side position loop PI controller. p (k) represents the location of the target load at time k. The load-side feedback position θ used by the motor for calculation m2 The error of (k).

[0036] Furthermore, if the host computer command is a motor-end position loop, then the motor-end position closed-loop calculation is initiated; the motor-end position closed-loop calculation is based on the target mechanical position of the motor-end position loop at time k. With actual feedback mechanical position The error is used to determine zero crossing, and the feedback mechanical position θ used in PI control is obtained. m1 (k), and then calculate and set the target rotational speed of the rotational speed loop at time k. The formula is as follows:

[0037]

[0038] Where, k ap k represents the proportional parameter of the motor end position loop PI controller. ai e represents the integral parameter of the motor end position loop PI controller. a (k) represents the motor end position and the target mechanical position at time k. Feedback mechanical position θ at the motor end m1 (k) error; actual feedback mechanical position The formula is P1(k) is the value of the first encoder at time k;

[0039] The zero-crossing determination method is as follows: Calculate the error between the target mechanical position and the actual feedback mechanical position of the motor end position loop as the first error, record the absolute value of the first error, and determine the sign of the first error; if it is positive, add one revolution to the actual feedback mechanical position and record it as the feedback position A; otherwise, subtract one revolution from the actual feedback mechanical position and record it as the feedback position A; calculate the error between the target mechanical position and the feedback position A of the motor end position loop as the second error, record the absolute value of the second error, and determine the magnitude of the absolute value of the second error and the absolute value of the first error; if the absolute value of the first error is smaller than the absolute value of the second error, use the actual feedback mechanical position of the motor as the final feedback position for the motor end position closed-loop calculation; otherwise, use the feedback position A as the final feedback position for the motor end position closed-loop calculation.

[0040] Furthermore, the closed-loop calculation of the speed loop includes:

[0041] The feedback speed is calculated based on the value of the first encoder, pos1, and the direction flag bit Flag1. The target current for the current loop closed-loop control is calculated and set based on the difference between the target speed and the feedback speed. Expressed as a formula Where, k sp k represents the proportional parameter of the speed loop PI controller. si e represents the integral parameter of the speed loop PI controller. s (k) represents the target rotational speed of the rotational loop at time k. Feedback speed ω from the speed loop at time k m The error of (k), where, T is the rotational speed calculation period. The actual feedback mechanical position at the motor end at time k.

[0042] Furthermore, the closed-loop calculation process for the current loop is as follows:

[0043] The current loop controller uses a proportional-integral controller to calculate the target voltages of the motor's d-axis and q-axis at time k. The formula is as follows:

[0044]

[0045] Among them, e d (k) represents the target current along the d-axis at time k. With actual current i d The error of (k), e d (i) represents the target current along the d-axis at time i. With actual current i d The error of (i), e q (k) represents the q-axis target current at time k. With actual current i q The error of (k), e q (i) represents the target q-axis current at time i. With actual current i q The error of (i);

[0046] The calculated target voltages along the d-axis and q-axis at time k. The Park inverse transform is performed, and the result of the transform is used as the input for space vector modulation. Then, SVPWM calculation is performed to obtain the PWM signal, which is sent to the intelligent drive module to drive the motor.

[0047] The advantages of this invention are:

[0048] (1) The drive controller of this invention integrates multiple modules. The encoder adopts an absolute position encoder. The absolute position encoder itself is ultra-thin and hollow, thereby effectively reducing the axial length of the joint. Through the design and installation of the drive controller and encoder, the installation space is reduced, the axial size of the joint is reduced, and it is easy to use. The control module has a built-in flexible joint motion control algorithm. Based on the received position information of the motor end and the load end, as well as the three-phase current and bus voltage of the motor, the flexible joint motion control algorithm is run and the control signal is output to the intelligent drive module, which effectively improves the control accuracy of the joint.

[0049] (2) In the algorithm of this invention, the motor and encoder parameters can be identified and calibrated online, which has strong applicability. The identification results are automatically stored in Flash and do not need to be recalibrated after power failure and restart. The automatic encoder calibration algorithm used traverses all motor electrical angle zero positions, which can greatly reduce the problem of inaccurate encoder calibration results caused by internal asymmetry of the motor. The zero-crossing judgment method used can avoid the problem of mechanical position jump caused by the inability to achieve full mechanical position tracking when the motor end or load end position is closed-loop controlled, which can effectively improve the control accuracy of the joint. Attached Figure Description

[0050] Figure 1This is a schematic diagram of the motion control system for a robot flexible joint module based on dual encoders disclosed in an embodiment of the present invention;

[0051] Figure 2 This is a schematic diagram of the signal connection of the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention;

[0052] Figure 3 This is a schematic diagram illustrating the execution steps of the flexible joint motion control algorithm in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention.

[0053] Figure 4 This is a schematic diagram showing the relationship between the phase axis and the direct axis of the motor under a single pole configuration in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention.

[0054] Figure 5 This is a schematic diagram of the logic flow of the flexible joint motion control algorithm in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention.

[0055] Figure 6 This is a schematic diagram of the load-side position loop logic flow of the flexible joint motion control algorithm in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention.

[0056] Figure 7 This is a flowchart of the motor end position loop logic of the flexible joint motion control algorithm in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention.

[0057] Figure 8 This is a flowchart of the rotational speed loop logic of the motion control algorithm for the flexible joint module of a robot based on dual encoders, as disclosed in the embodiments of the present invention.

[0058] Figure 9 This is a current loop logic flowchart of the flexible joint motion control algorithm in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention.

[0059] Figure 10 This is a flowchart of the zero-crossing judgment logic of the flexible joint motion control algorithm in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention.

[0060] Figure 11 This is a block diagram of the flexible joint motion control algorithm in the motion control system of the robot flexible joint module based on dual encoders disclosed in the embodiments of the present invention. Detailed Implementation

[0061] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0062] like Figure 1 and Figure 2 As shown, the motion control system of the robot flexible joint module based on dual encoders includes a first encoder 1, a motor 2, a harmonic reducer 3, an elastic element 4, a second encoder 5, a joint shell 6, a drive controller 7, and a load-end output shaft. The first encoder 1 and the second encoder 5 are absolute position encoders, and the motor 2 is a frameless torque motor 2. The first encoder 1 and the second encoder 5 both include a magnetic grating and an encoder chip. Both the magnetic grating and the encoder chip are ring-shaped, with the encoder chip fitted over the magnetic grating. The magnetic grating of the first encoder 1 is fitted onto and fixedly connected to the shaft of the motor 2. The shaft of the motor 2 is also coaxially and fixedly connected to the input shaft of the harmonic reducer 3. The output shaft of the harmonic reducer 3 is fixedly connected to the elastic element 4. The elastic element 4 is fixedly connected to the magnetic grating of the second encoder 5. The magnetic grating of the second encoder 5 is also fixedly connected to the load-end output shaft. The joint housing 6 is a hollow structure with through holes at both ends. The shaft of the motor 2 passes through the through hole at one end of the joint housing 6 and connects to an external device. The load-end output shaft passes through the through hole at the other end of the joint housing 6 and acts on an object. The encoder chip of the first encoder 1, the housing of the motor 2, the encoder chip of the second encoder 5, and the drive controller 7 are all fixed on the joint housing 6.

[0063] Continue reading Figure 2The drive controller 7 includes a communication module, a control module, and an intelligent drive module connected in sequence. The intelligent drive module is electrically connected to the motor 2. The first encoder 1 and the second encoder 5 are both connected to the control module through the communication module. The intelligent drive module includes a current sensor, a bus voltage detection unit, a three-phase inverter bridge and its gate drive unit. The current sensor is used to detect the three-phase current of the motor 2, the bus voltage detection unit is used to detect the bus voltage, and the three-phase inverter bridge and its gate drive unit are used to drive the motor 2. The first encoder 1 and the second encoder 5 provide feedback on the position information of the motor end and the load end to the control module. The intelligent drive module detects the three-phase current and bus voltage of the motor 2 and sends them to the control module. At the same time, it receives and amplifies the control signal output by the control module to drive the motor 2. The control module runs a flexible joint motion control algorithm based on the received position information of the motor end and the load end, as well as the three-phase current and bus voltage of the motor 2, and outputs a control signal to the intelligent drive module. (Continue reading...) Figure 2 The motion control system also includes a power supply module, an interface module, and a host computer. The power supply module manages the bus voltage and outputs multiple different voltages to power various modules. The communication module connects to the power supply module and the host computer through the interface module.

[0064] like Figure 3 As shown, the flexible joint motion control algorithm includes the following steps:

[0065] S1. Correct the zero reference value of the current sensor in the intelligent drive module; In this embodiment, the current sensor of the intelligent drive module is used to detect the three-phase current of the frameless torque motor 2. The flexible joint needs to correct the zero reference value of the current sensor every time it is powered on, in order to reduce the impact of the zero drift of the current sensor on the control accuracy of the motor 2 and ensure that the reading of the current sensor can accurately reflect the three-phase current of the motor 2.

[0066] S2. Identify the stator resistance and direct-axis inductance of motor 2, and calculate the current loop control parameters, including the proportional parameter k of the PI controller. cp The integral parameter k of the PI controller ci The specific process is as follows:

[0067] S21. Stator resistance identification;

[0068] Stator resistance is identified using the DC volt-ampere method, such as... Figure 4 As shown, using Figure 1 The drive controller 7 applies a DC voltage U to the motor 2, with phase A being positive and phases B and C being negative, causing the rotor of the motor 2 to reach a fixed position, at which point the A-phase shaft of the motor 2 coincides with the direct shaft (d-axis); the A-phase current is collected N times, and the average value I of the N currents is calculated. N Through R s =2U / 3IN The stator resistance R is calculated. s ; Figure 4 The straight axis is the d-axis, and the intersection axis is the q-axis.

[0069] S22, Direct-axis inductor identification;

[0070] Calculate the direct-axis inductance value based on the d-axis voltage equation of the permanent magnet synchronous motor 2 in the dq rotating coordinate system. The d-axis voltage equation is:

[0071]

[0072] In the formula, R s i d i q L d L q ω and ω represent the stator resistance, direct-axis current of motor 2, quadrature-axis current of motor 2, direct-axis inductance, AC inductance, and electric angular velocity, respectively.

[0073] Since the rotor of motor 2 is stationary, the electric angular velocity ω of motor 2 is zero, and the voltage equation simplifies to:

[0074]

[0075] As one implementation method, ignoring the voltage drop across the resistor, the voltage equation is further simplified to:

[0076]

[0077] Therefore, the specific process for identifying the parameters of the direct-axis inductor is as follows:

[0078] pass Figure 1 The drive controller 7 applies a square wave voltage. From the voltage equation (3), it can be seen that the current rises approximately at a constant slope, which is k = u. d / L d The direct-axis inductance value L can be calculated after measuring the slope k. d .

[0079] As one implementation method, based on the step response characteristics of the inertial element, it can be known that given u d A step signal is used to obtain the current i. d The current response is

[0080]

[0081] Therefore, when t = L d / R s hour,

[0082] After identifying the stator resistance, the steady-state current I is calculated. sThe drive controller 7 injects a pulse voltage into the motor 2 and records the direct-axis current i of the motor 2. d The time t required to reach 0.632 times the steady-state current is calculated according to the formula L. d =tR s The direct-axis inductance value is calculated, and the average value is obtained from multiple calculations to obtain the direct-axis inductance identification result.

[0083] S23. Based on the stator resistance and direct-axis inductance of motor 2, calculate the control parameters of the current loop controller in the flexible joint motion control algorithm, and store the control parameters in Flash. The calculation formula for the control parameters is as follows:

[0084]

[0085] Where, k cp k represents the proportional parameter of the PI controller. ci This represents the integral parameter of the PI controller, and f represents the current loop control frequency.

[0086] S3. Calibrate the difference pos1 between the zero position of the first encoder 1 and the electrical angle zero of the motor 2; the specific process is as follows:

[0087] S31. Record the value P of the first encoder 1 when the electrical angle of motor 2 is zero. 11 The drive controller 7 applies voltage to the motor 2, causing the rotor of the motor 2 to rotate to an electrical angle of 2π in steps of π / 3 electrical angles each time. The value P of the first encoder 1 at this time is recorded. 12 According to P 11 With P 12 The difference is used to determine whether the positive direction of rotation of motor 2 is consistent with the direction of the increase of the value of the first encoder 1. If they are consistent, the direction flag 1 is 1; otherwise, the direction flag 1 is -1. The direction flag 1 is used to calculate the electrical angle and mechanical position of the motor end in the subsequent flexible joint motion control algorithm.

[0088] S32. Apply voltage to motor 2 via drive controller 7, causing the rotor of motor 2 to rotate to a mechanical angle of 2π in steps of π / 3 electrical angles each time. Record the value P of the first encoder 1 when the rotor reaches each electrical angle of 0 (2π). 11 P 12 ...P 1p Where p is the number of pole pairs of motor 2; let P be the sum of the p values ​​of the first encoder 1. sum =P 11 +P 12 +……+P 1p Wherein, electrical angle = mechanical angle × number of pole pairs of the motor;

[0089] S33. If the maximum single-turn count of the first encoder 1 is CPR, then the difference pos1 between its zero position and the electrical angle zero of the motor 2 is:

[0090] S34. Store pos1 and direction flag 1 into Flash.

[0091] It should be noted that this calibration step should be performed before the flexible joint is assembled into the robot or robotic arm; this step should not be performed after assembly.

[0092] S4. Correct the difference pos2 between the zero position of the second encoder 5 and the mechanical angle zero at the joint output end, where the joint output end is the load end; the specific process is as follows:

[0093] S41. Record the value P of the second encoder 5 when the electrical angle of motor 2 is zero. 21 The drive controller 7 applies voltage to the motor 2, causing the rotor of the motor 2 to rotate to an electrical angle of 2π in steps of π / 3 electrical angles each time. The value P of the second encoder 5 at this time is recorded. 22 According to P 21 With P 22 The difference is used to determine whether the positive direction of the rotation of motor 2 is consistent with the direction of the increase of the value of the second encoder 5. If they are consistent, the direction flag 2 is 1; otherwise, the direction flag 2 is -1. The direction flag 2 is used to calculate the mechanical position of the load end in the flexible joint motion control algorithm.

[0094] S42. After the flexible joint module is assembled into the robot or robotic arm, the configuration of the robot or robotic arm is set to the state where the mechanical angle at the end of the flexible joint is zero. The value of the second encoder 5 at this time is recorded, which is the difference pos2 between the zero position of the second encoder 5 and the zero mechanical angle at the joint output end.

[0095] S43. Store pos2 and direction flag 2 into Flash.

[0096] S5, such as Figures 5 to 11 As shown, the current closed-loop calculation is initiated first. Then, according to the instructions from the host computer, the control modes of the speed loop, motor end position loop, and load end position loop are entered respectively to perform corresponding closed-loop control. During the control process, the calculation result of the load end position loop is sent to the motor end position loop or speed loop, the calculation result of the motor end position loop is sent to the speed loop, and the calculation result of the speed loop is sent to the current loop. The current loop calculation result undergoes inverse Park transform, and the result of the transform is used as the input of space vector modulation, which then performs SVPWM calculation to obtain the PWM signal, which is sent to the intelligent drive module to drive motor 2 to move. The specific process is as follows:

[0097] Continue reading Figure 11If the host computer command is for the load-side position loop, then determine whether the motor-side position loop needs to be run. If so, start the motor-side position loop calculation first, then start the load-side position loop calculation, calculate and set the target mechanical position of the motor-side position loop. Otherwise, directly start the load-side position loop calculation, calculate and set the target speed of the speed loop.

[0098]

[0099] The load-side position closed-loop calculation includes calculating the actual load-side feedback position of motor 2 based on the values ​​P2(k) and pos2 of the second encoder 5 and the direction flag 2. The zero-crossing judgment is performed based on the difference between the target load end position and the actual load end feedback position of motor 2 to obtain the load end feedback position θ used for calculation. m2 (k), and then the target rotational speed of the rotational speed loop at time k is calculated. Or the target mechanical position around the motor end position The formula is as follows:

[0100]

[0101] Among them, the motorless end position ring (N) corresponds to Figure 11 After selecting the N branch of the motor-side position loop and starting the load-side position loop, calculate and set the target speed of the speed loop. There is a motor end position ring (Y) corresponding to Figure 11 The Y-branch selected in the motor-end position loop is used to calculate and set the target mechanical position of the motor-end position loop after starting the load-end position loop. k pp k represents the proportional parameter of the load-side position loop PI controller. pi e represents the integral parameter of the load-side position loop PI controller. p (k) represents the location of the target load at time k. The load-side feedback position θ used for calculation with motor 2 m2 The error of (k).

[0102] The method for zero-crossing judgment in the closed-loop calculation of the load end position is the same as the method for zero-crossing judgment in the closed-loop calculation of the motor end position below. The only difference is that the target mechanical position and the actual feedback mechanical position of the motor end position loop in the whole process are replaced with the target position of the load end and the actual feedback position of the load end of motor 2, respectively. The execution logic is exactly the same. For details, please refer to the zero-crossing judgment method below. It will not be repeated here.

[0103] If the host computer command is a motor end position loop, then the motor end position closed-loop calculation is initiated; the motor end position closed-loop calculation is based on the target mechanical position of the motor end position loop at time k. With actual feedback mechanical position The error is used to determine zero crossing, and the feedback mechanical position θ used in PI control is obtained. m1 (k), and then calculate and set the target rotational speed of the rotational speed loop at time k. The formula is as follows:

[0104]

[0105] Where, k ap k represents the proportional parameter of the motor end position loop PI controller. ai e represents the integral parameter of the motor end position loop PI controller. a (k) represents the motor end position and the target mechanical position at time k. Feedback mechanical position θ at the motor end m1 (k) error; actual feedback mechanical position The formula is P1(k) is the value of the first encoder 1 at time k;

[0106] The zero-crossing determination method is as follows: Calculate the error between the target mechanical position and the actual feedback mechanical position of the motor end position loop as the first error, record the absolute value of the first error, and determine the sign of the first error; if it is positive, add one revolution to the actual feedback mechanical position and record it as the feedback position A; otherwise, subtract one revolution from the actual feedback mechanical position and record it as the feedback position A; calculate the error between the target mechanical position and the feedback position A of the motor end position loop as the second error, record the absolute value of the second error, and determine the magnitude of the absolute value of the second error and the absolute value of the first error; if the absolute value of the first error is smaller than the absolute value of the second error, use the actual feedback mechanical position of motor 2 as the final feedback position for motor end position closed-loop calculation; otherwise, use the feedback position A as the final feedback position for motor end position closed-loop calculation.

[0107] The closed-loop calculation of the speed loop includes:

[0108] The feedback speed is calculated based on the value of the first encoder 1, pos1, and the direction flag bit Flag1. The target current for the current loop closed-loop control is calculated and set based on the difference between the target speed and the feedback speed. Expressed as a formula Where, k sp k represents the proportional parameter of the speed loop PI controller. si e represents the integral parameter of the speed loop PI controller. s (k) represents the target rotational speed of the rotational loop at time k. Feedback speed ω from the speed loop at time k m The error of (k), where, T is the rotational speed calculation period. The actual feedback mechanical position at the motor end at time k.

[0109] The closed-loop calculation process for the current loop is as follows:

[0110] The current loop controller uses a proportional-integral controller to calculate the target voltages of motor 2 along the d and q axes at time k. The formula is as follows:

[0111]

[0112] Among them, e d (k) represents the target current along the d-axis at time k. With actual current i d The error of (k), e d (i) represents the target current along the d-axis at time i. With actual current i d The error of (i), e q (k) represents the q-axis target current at time k. With actual current i q The error of (k), e q (i) represents the target q-axis current at time i. With actual current i q The error of (i);

[0113] The calculated target voltages along the d-axis and q-axis at time k. The Park inverse transform is performed, and the result of the transform is used as the input of space vector modulation. Then, SVPWM calculation is performed to obtain the PWM signal, which is sent to the intelligent drive module to drive motor 2 to move.

[0114] Through the above technical solutions, the present invention reduces the installation space and shrinks the axial dimension of the joint by designing and installing the drive controller 7 and the position encoder; it provides a flexible joint control method based on dual position encoders, improves the positioning accuracy of the joint end, and combines automatic zero-position calibration of the encoder and parameter identification of the motor 2, so that the drive controller 7 can be applied to a variety of different rotary joints.

[0115] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A motion control system for a robot flexible joint module based on dual encoders, characterized in that, The flexible joint module includes a drive controller, a motor, a first encoder, and a second encoder. The first and second encoders are absolute position encoders. The drive controller includes a communication module, a control module, and an intelligent drive module connected in sequence. The intelligent drive module is electrically connected to the motor. Both the first and second encoders are connected to the control module via the communication module. The first and second encoders provide position information from the motor and load ends to the control module. The intelligent drive module detects the three-phase current and bus voltage of the motor and sends them to the control module. Simultaneously, it receives and amplifies the control signal output by the control module to drive the motor. Based on the received position information from the motor and load ends, as well as the three-phase current and bus voltage of the motor, the control module runs a flexible joint motion control algorithm and outputs a control signal to the intelligent drive module. The flexible joint motion control algorithm includes the following steps: Step 1: Calibrate the zero reference value of the current sensor in the intelligent drive module; Step 2: Identify the motor stator resistance and direct-axis inductance, and calculate the current loop control parameters, including the proportional parameters of the PI controller. k cp Integral parameters of the PI controller k ci ; Step 3: Calibrate the difference between the zero position of the first encoder and the zero electrical angle of the motor. pos 1; Step 4: Correct the difference between the zero position of the second encoder and the mechanical angle zero at the joint output end. pos 2. The joint output terminal is the load terminal; Step 5: First, start the current closed-loop calculation. Then, according to the host computer instructions, enter the speed loop, motor end position loop, and load end position loop control modes respectively to perform corresponding closed-loop control. During the control process, the calculation result of the load end position loop is sent to the motor end position loop or speed loop, the calculation result of the motor end position loop is sent to the speed loop, the calculation result of the speed loop is sent to the current loop, the current loop calculation result is subjected to Park inverse transformation, and the transformation result is used as the input of space vector modulation, and then SVPWM calculation is performed to obtain the PWM signal, which is sent to the intelligent drive module to drive the motor to move.

2. The motion control system for a robot flexible joint module based on dual encoders according to claim 1, characterized in that, The flexible joint module also includes a harmonic reducer, an elastic element, a load-side output shaft, and a joint housing. Both the first and second encoders include a magnetic grating and an encoder chip, both of which are annular. The encoder chip is fitted over the magnetic grating. The magnetic grating of the first encoder is fitted onto and fixedly connected to the motor shaft. The motor shaft is also coaxially and fixedly connected to the input shaft of the harmonic reducer. The output shaft of the harmonic reducer is fixedly connected to the elastic element, and the elastic element is fixedly connected to the magnetic grating of the second encoder. The magnetic grating of the second encoder is also fixedly connected to the load-side output shaft. The joint housing has a hollow structure and through holes at both ends. The motor shaft passes through a through hole at one end of the joint housing and connects to an external device. The load-side output shaft passes through a through hole at the other end of the joint housing and acts on an object. The encoder chip of the first encoder, the housing of the motor, the encoder chip of the second encoder, and the drive controller are all fixed on the joint housing. The intelligent drive module includes a current sensor, a bus voltage detection unit, a three-phase inverter bridge and its gate drive unit. The current sensor is used to detect the three-phase current of the motor, the bus voltage detection unit is used to detect the bus voltage, and the three-phase inverter bridge and its gate drive unit are used to drive the motor to move.

3. The motion control system for a robot flexible joint module based on dual encoders according to claim 1, characterized in that, Step two includes: Step 21: Stator resistance identification; Use the drive controller to convert the DC voltage U When applied to the motor, phase A is positive, and phases B and C are negative, causing the motor rotor to reach a fixed position where the shaft of phase A coincides with the direct shaft; data collection. N Secondary phase A current, and calculate N average of secondary current I N ,pass R s =2 U / 3 I N The stator resistance was calculated. R s ; Step 22: Direct-axis inductor identification; After identifying the stator resistance, the steady-state current is calculated. I s The drive controller injects pulse voltage into the motor and records the direct-axis current of the motor. i d Time required to reach 0.632 times the steady-state current t According to the formula L d = tR s The direct-axis inductance value is calculated, and the average value of multiple calculations is used to obtain the direct-axis inductance identification result. Step 23: Based on the stator resistance and direct-axis inductance of the motor, calculate the control parameters of the current loop controller in the flexible joint motion control algorithm, and store the control parameters in Flash. The calculation formula for the control parameters is as follows: in, k cp This represents the proportional parameter of the PI controller. k ci This represents the integral parameter of the PI controller. f This indicates the current loop control frequency.

4. The motion control system for a robot flexible joint module based on dual encoders according to claim 1, characterized in that, Step three includes: Step 31: Record the value of the first encoder when the motor electrical angle is zero. P 11 The drive controller applies voltage to the motor, causing the motor rotor to rotate to an electrical angle of 2π in steps of π / 3 electrical angles each time. The value of the first encoder at this point is recorded. P 12 ;according to P 11 and P 12 The difference is used to determine whether the positive direction of motor rotation is consistent with the direction of the increase in the value of the first encoder. If they are consistent, the direction flag 1 is 1; otherwise, the direction flag 1 is -1. Step 32: Apply voltage to the motor via the drive controller, causing the motor rotor to rotate to a mechanical angle of 2π in increments of π / 3 electrical angles each time. Record the value of the first encoder when the rotor reaches each electrical angle of 0. P 11 , P 12 ... P 1p ,in p Let be the number of pole pairs of the motor; denote the first encoder's... p The sum of the values ​​is P sum = P 11 + P 12 +……+ P 1p Wherein, electrical angle = mechanical angle × number of pole pairs of the motor; Step 33: If the maximum single-turn count of the first encoder is CPR, then the difference between its zero position and the zero electrical angle of the motor is... pos 1 is ; Step 34, pos 1. The direction flag (Flag1) is stored in Flash.

5. The motion control system for a robot flexible joint module based on dual encoders according to claim 1, characterized in that, Step four includes: Step 41: Record the value of the second encoder when the motor electrical angle is zero. P 21 The drive controller applies voltage to the motor, causing the motor rotor to rotate to an electrical angle of 2π in increments of π / 3 electrical angles each time. The value of the second encoder at this point is recorded. P 22 ;according to P 21 and P 22 The difference is used to determine whether the positive direction of the motor rotation is consistent with the direction of the increase in the value of the second encoder. If they are consistent, the direction flag 2 is 1; otherwise, the direction flag 2 is -1. Step 42: After assembling the flexible joint module onto the robot or robotic arm, set the robot or robotic arm configuration to a state where the mechanical angle at the end of the flexible joint is zero. Record the value of the second encoder at this time, which is the difference between the zero position of the second encoder and the zero mechanical angle at the joint output end. pos 2; Step 43, pos 2. The direction flag (Flag2) is stored in Flash.

6. The motion control system for a robot flexible joint module based on dual encoders according to claim 1, characterized in that, If the host computer command is for the load-side position loop, then determine whether the motor-side position loop needs to be run. If so, start the motor-side position loop calculation first, then start the load-side position loop calculation, calculate and set the target mechanical position of the motor-side position loop. Otherwise, directly start the load-side position loop calculation, calculate and set the target speed of the speed loop. ; The load-side position closed-loop calculation includes calculations based on the values ​​from the second encoder. P 2( k ), pos 2. The direction flag (Flag2) is used to calculate the load-side feedback position of the motor. The zero-crossing judgment is performed based on the difference between the target load end position and the load end feedback position used by the motor for calculation, thus obtaining the load end feedback position used for calculation. Then calculate the first k target rotation speed at any time Or the target mechanical position around the motor end position The formula is as follows: in, k pp This indicates the proportional parameter of the load-side position loop PI controller. k pi This represents the integral parameter of the load-side position loop PI controller. e p ( k ) indicates the first k Target load location at any time Feedback position of the load end used for calculation with the motor The error.

7. The motion control system for a robot flexible joint module based on dual encoders according to claim 6, characterized in that, If the host computer command is a motor end position loop, then the motor end position closed-loop calculation is initiated; the motor end position closed-loop calculation is based on the first... k Momentary motor end position ring target mechanical position With actual feedback mechanical position The error is used to determine zero crossing, and the feedback mechanical position is obtained for use in PI control. , and then calculate and set the first k target rotation speed at any time The formula is as follows: in, k ap This indicates the proportional parameter of the PI controller at the motor end position. k ai This represents the integral parameter of the PI controller at the motor end position. e a ( k ) indicates the first k Momentary motor end position ring target mechanical position Feedback mechanical position at the motor end Error; actual feedback mechanical position The formula is , P 1( k ) is the first encoder k The numerical value at any given time; The zero-crossing determination method is as follows: Calculate the error between the target mechanical position and the actual feedback mechanical position of the motor end position loop as the first error, record the absolute value of the first error, and determine the sign of the first error; if it is positive, add one revolution to the actual feedback mechanical position and record it as the feedback position A; otherwise, subtract one revolution from the actual feedback mechanical position and record it as the feedback position A; calculate the error between the target mechanical position and the feedback position A of the motor end position loop as the second error, record the absolute value of the second error, and determine the magnitude of the absolute value of the second error and the absolute value of the first error; if the absolute value of the first error is smaller than the absolute value of the second error, use the actual feedback mechanical position of the motor as the final feedback position for the motor end position closed-loop calculation; otherwise, use the feedback position A as the final feedback position for the motor end position closed-loop calculation.

8. The motion control system for a robot flexible joint module based on dual encoders according to claim 7, characterized in that, The closed-loop calculation of the speed loop includes: Based on the value of the first encoder, pos 1. The direction flag (Flag1) is used to calculate the feedback speed. Based on the difference between the target speed and the feedback speed, the target current for the current loop closed-loop control is calculated and set. Expressed by the formula: ,in, k sp This represents the proportional parameter of the speed loop PI controller. k si This represents the integral parameter of the speed loop PI controller. e s ( k ) indicates the first k target rotation speed at any time With the k Time-of-flight speed loop feedback speed The error, of which, , T For the rotational speed calculation period, For the first k The actual mechanical position is fed back from the motor end at all times.

9. The motion control system for a robot flexible joint module based on dual encoders according to claim 8, characterized in that, The closed-loop calculation process for the current loop is as follows: The current loop controller uses a proportional-integral controller to calculate the first... k Target voltages of the motor on the d and q axes at any given time , The formula is as follows: in, e d ( k ) indicates the first k d-axis target current at time With actual current The error, e d ( i ) indicates the first i d-axis target current at time With actual current The error, e q ( k ) indicates the first k Target current along the q-axis at time 1 With actual current The error, e q ( i ) indicates the first i Target current along the q-axis at time 1 With actual current The error; For the calculated first k Target voltages on the d-axis and q-axis at time points , The Park inverse transform is performed, and the result of the transform is used as the input for space vector modulation. Then, SVPWM calculation is performed to obtain the PWM signal, which is sent to the intelligent drive module to drive the motor.

Images