Reducing motion error
Through parameter vector programming and feedback correction of sinusoidal frequency components, the problems of joint rotation speed fluctuations and position deviation of robot manipulators are solved, achieving higher control accuracy and accuracy.
Patent Information
- Application Number
- CN202280096129.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2022-05-17
- Publication Date
- 2024-12-27
AI Technical Summary
The rotation speed fluctuations and position deviations caused by manufacturing inaccuracy of the joints in the robot manipulator lead to motion errors, affecting control accuracy.
The motion correction signal is generated through parameter vector programming, the sinusoidal frequency component is used for feedback correction, the motor speed is adjusted to reduce motion errors, high-pass filtering is used to suppress the influence of external forces, and real-time correction is performed using the controller and processor.
It effectively reduces the motion error of robot joints, improves the accuracy of position and speed control, and enhances the accuracy of robot arms.
Smart Images
Figure CN119212830A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to methods and apparatus for reducing motion errors when controlling the movement of joints in a robotic manipulator. Background Art
[0002] The robotic manipulator includes a plurality of links that are coupled to each other through rotatable joints, to a base or to an end effector. The rotation of such joints is usually driven by a motor via a reduction gear. The clearance and inaccuracy in manufacturing the motor and gears can affect the accuracy of control of the position and speed of the end effector at the distal end of the robotic arm. Harmonic drive gears are widely used as reduction gears for robots because they are almost free of clearance, but unavoidable inaccuracies (such as eccentricity of gears or splines) still cause the rotational speed of the joint to fluctuate, even if its motor can be driven at a completely constant speed, so that the position of the joint deviates from the position expected by people based on the position of the motor and the transmission ratio of the reduction gear. This deviation is usually called motion error or transmission error. Summary of the invention
[0003] It is an object of the present invention to provide a simple and cost-effective way of reducing kinematic errors in robot joints.
[0004] According to one aspect of the invention, the object is achieved by a method for reducing a kinematic error in a j-th joint between a distal part and a proximal part of a manipulator, the joint having a motor associated therewith, the motor being mounted in one of the parts and coupled to the other of the parts for driving the distal part in rotation relative to the proximal part via a reduction gear, the method comprising the following steps:
[0005] a. Providing a motion correction generator programmable using a parameter vector and outputting a motion correction based on the parameter vector and a position input, the motion correction being a sum of at least one sinusoid having:
[0006] - a frequency, which is the frequency of rotation of the motor or the frequency at which periodic events occur in the reduction gear at a frequency higher than the frequency of rotation of the distal part or an integer multiple thereof; and
[0007] - an amplitude and a phase defined by said parameter vector;
[0008] b. outputting a first drive control signal specifying a motor speed to the motor, the motor speed being a sum of a standard speed and a first motion correction output by a motion correction generator based on a first parameter vector;
[0009] c. extracting at least one frequency component having a frequency of at least one sinusoid from an evaluation signal received from a joint in response to a first drive control signal, and determining a first feedback vector defining a first feedback amplitude and a first feedback phase of said frequency component;
[0010] d. outputting a second drive control signal specifying a motor speed to the motor, the motor speed being the sum of the standard speed and the second motion correction output by the motion correction generator based on the second parameter vector;
[0011] e. extracting a frequency component having a frequency of at least one sinusoid from an evaluation signal received from a joint in response to a second drive control signal, and determining a second feedback vector defining a second feedback amplitude and a second feedback phase of the frequency component;
[0012] f. subtracting the first feedback vector from the second feedback vector to obtain a feedback difference vector;
[0013] g. Find a transformation that transforms the feedback difference vector into the difference between the second parameter vector and the first parameter vector;
[0014] h. applying the transformation to the second feedback vector to obtain a third parameter vector; and
[0015] i. Program the motion correction generator using the third parameter vector.
[0016] In a preferred embodiment, the above process is simplified by selecting a first parameter vector to define the amplitude of at least one sinusoid as zero. A sinusoid can be defined as a weighted sum of a sine function and a cosine function; in this case, the weighting coefficients of both must be zero. Alternatively, it can be defined as a sine or cosine function with amplitude and phase; when the amplitude here is zero, the phase has no physical meaning and can be assigned any value.
[0017] In order to facilitate the processing of the evaluation signal, the reference speed should be constant, or at least substantially constant.
[0018] The standard speed can be considered to be essentially constant if the influence of external forces acting on the distal part, for example due to the weight of the distal part driving or hindering the movement of the distal part, on the evaluation signal can be suppressed by high-pass filtering the evaluation signal. Due to the reduction gear, the rotation speed of the motor will be much higher than the rotation speed of the joint. Since the external forces are generally proportional to the sine curve of the joint angle, these external forces can be suppressed by setting a cut-off frequency that is lower than the motor frequency and higher than the motor frequency multiplied by the reduction gear reduction ratio. The standard speed or its harmonics should correspond to the resonant frequency of the manipulator, that is, the motor frequency required to drive the distal part at the standard speed or a harmonic (preferably the second harmonic) should be equal to the resonant frequency.
[0019] This speed does not have to be known in advance; on the contrary, finding it can be a preparatory step of the method of the present invention. Specifically, in the preparatory stage of the method, the distal part can be controlled to rotate at different speeds, and the speed at which the vibration of the manipulator is stronger can be selected as the standard speed.
[0020] It can be assumed that, when the distal part repeatedly performs the same movement at different speeds, the robot controller is trying to find a critical speed that changes from one iteration to the next, with the speed difference between successive iterations occasionally changing its sign, and / or the amount of the speed difference decreasing with each sign change.
[0021] Since the moment of inertia of the manipulator or a component of the manipulator varies depending on the configuration, and for the method of the present invention it is desirable to have a low critical speed, the distal portion can be controlled to assume a configuration that maximizes its moment of inertia and should be maintained while performing the method.
[0022] The evaluation signal may represent any measurable quantity that is susceptible to fluctuations due to imperfections in the motor or reduction gear. It may be the rotational speed of the distal member itself as measured, for example, by a dedicated angle sensor built into the manipulator for measuring the rotation of the joint on the downstream side of the reduction gear, a laser scanner or other optical sensing means for observing the manipulator from the outside. According to a preferred embodiment, the evaluation signal represents the motor torque, since suitable sensing circuitry exists in many conventional manipulators, so when the method of the present invention utilizes this circuitry, it can be implemented in these manipulators by a control software update.
[0023] When the harmonic drive gear is used as a reduction gear, its wave generator is usually connected to the motor output shaft, while its flexible wheel is connected to the output shaft of the reduction gear, and the rigid wheel is fixed. c And the number of teeth of the flexible wheel is i f When the wave generator is in the position relative to the flexible wheel, the wave generator and the motor output shaft are each i f / ic After each rotation, they will become the same, causing mi to the rotation frequency of the motor f / i c times the contribution of the motion error at i, where m is a small integer. f Slightly less than i c , so the frequency of the flexspline and rigid wheel contribution to the motion error is often slightly different from the frequency of the motor and wave generator.
[0024] Experiments have shown that for most harmonic drive gears, the most important contribution of the flexspline and rigid spline is at 2 i of the rotational speed. f / i c Times.
[0025] To take into account the various sources contributing to the motion error, in most practical cases the motion correction will be the sum of two or more sinusoids.
[0026] The best way to provide a motion correction generator is to program a processor so that it can operate as a motion correction generator, specifically, to update the control processor of a conventional manipulator so that it can operate as a motion correction generator, or to make the third parameter vector smaller so that it can operate as a motion correction generator whenever maintenance or repair work is performed at the corresponding joint.
[0027] According to another aspect of the present invention, the object is achieved by a robot system, the robot system comprising:
[0028] - a manipulator having a proximal portion, a distal portion, a joint connecting the proximal portion and the distal portion, and a motor for driving the joint to rotate; and
[0029] - A controller connected to said electric machine and adapted to perform at least steps b to i of the method described above.
[0030] According to another aspect, the object is achieved by a computer software product comprising a plurality of instructions which, when executed by a processor, cause the processor to perform at least steps b to i of the method described above. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] Other features and advantages of the present invention will become apparent from the following description of embodiments with reference to the accompanying drawings.
[0032] Figure 1 is a block diagram of a robot system implementing the present invention;
[0033] Figure 2 yes Figure 1 A perspective view of a manipulator of the system;
[0034] Figure 3 is the cross section of the harmonic drive gear;
[0035] Figure 4 is the pose the manipulator takes when executing the method on one of its joints; and
[0036] Figure 5 is the pose the manipulator takes when executing the method on another of its joints. DETAILED DESCRIPTION
[0037] Figure 1 is a block diagram of a robot system including an articulated robot arm 1 and a controller 2 to which the method of the present invention may be applied. Figure 2 The robot arm 1 shown in more detail in FIG. 1 has a fixed base 3 fixed to a support and a plurality of links 4 j , where j = 1, 6, the multiple connecting rods 4 j are rotatably connected to each other and connected by a joint 5 j Connect to base 3. Connect to connecting rod 4 4 , 4 5 Joints 5 5 Hidden in the connecting rod 4 5 Back. Each joint 5 j Both make the connecting rod 4 on the distal side j Around axis 6 j Rotation. Axis 6 1 is vertical, axis 6 2 and 6 3 is horizontal; the orientation of the other axes is different. Each joint 5j has a motor associated with it for driving its rotation; the motor can be arranged in a member on the proximal side of the joint and connected to a member on its distal side, or vice versa. Figure 2 In the robot arm, joint 5 1 , 5 2 , 5 3 The motors are located in the components on the proximal side, namely, the base 3 and the connecting rod 4. 1 , 4 5 Middle; Joint 5 3 , 5 4 and 5 5 The motors on the distal end are located on the connecting rod 4 3 , 4 4 and 4 5 middle.
[0038] Figure 1 Shown with joint 5 2 , 5 3 , 5 4 Associated motor 9 2 , 93 , 9 4 and connect these motors to the connecting rod 4 respectively 2 , 4 2 and 4 3 Reduction gear 10 2 , 10 3 , 10 4 . Each motor 9 j All are equipped with current sensor 7 j , used to measure its winding current. Since the sensor 7 j The measured data represent motor 9 j The applied torque, so they are referred to as torque data τ below mot ·. Rotary transformer 8 j are placed at each joint 5 j In order to measure its rotation angle
[0039] Reduction gear 10 j It is a harmonic gear, such as Figure 3 , where the wave generator 16 is rigidly coupled to the motor 9 j The output shaft 17 of the flex spline 18 is pressed to partially mesh with the rigid spline 19 surrounding it. When the number of teeth between the rigid spline 19 and the flexible spline 18 is i c and i f When gear 10 j The transmission ratio is Since the flexible wheel 18 rotates in the opposite direction to the wave generator 16, the wave generator 16 rotates 2πl. f / i c (slightly less than one circle), the two currently touching points will meet again.
[0040] The controller 2 includes a trajectory generator 11 for moving the target object to the specific joint 5 at an appropriate timing. j Associated joint controller 12 j Output Assuming that there are no kinematic errors in these joints, link 4 6 Or the position and velocity commands that may be required to follow a predetermined trajectory at a reference point of a tool mounted thereon. Each joint controller 12 j Also associated with it is a method for calculating the motion error correction δq ke Motion Error Calculator 13 j and a command q for superimposing the motion error correction on the output of the trajectory generator 11 mot，j . and provide the corrected command q mot，j =q mot，j +δq ke , Adder 14j , 15 j , as explained in detail below.
[0041] Each joint can be treated 5 times at a time j All perform the method of the present invention.
[0042] In the first step (S1, see Figure 5 ), the manipulator 1 is brought into a state where the connecting rod 4 j to 4 6 The center of gravity is as far away from the axis 6j as possible so that it is at the joint 5 j The configuration that maximizes the resonant frequency of the components on the distal side, such as Figure 4 As shown, where j = 2. The counting index i is set to 1, and the fluctuation reference F 0 is set to 0, and the step width is initialized.
[0043] Set (S2) Motor 9 j Rotation speed The trajectory generator 11 supplies the motor 9 with j The output has a constant rate of change Angle command To control the nominal constant speed around axis 6 j rotation.
[0044] In step 3, when joint 5 j When the motor 9 rotates at the speed set in step S2, j and reduction gear 10 j The defect causes sensor 7 j The current fluctuation detected. Record (S4) the measured value F of this fluctuation i , for example, the standard deviation of the current measured while performing the rotation.
[0045] If it is found in step S5 that F i -F i-1 is positive and If it is higher than a predetermined threshold, then in step S6, i is incremented and Increment And the method returns to step S3. If it is found that F i -F i-1 If negative, before continuing to step S6, Multiply by a factor ε between 0 and -1 (S7). Successive multiplications by ε will result in When it falls below the threshold, the fluctuation F reaches its maximum value, and it can be assumed that It is joint 5 j critical speed.
[0046] Each time step S3 is performed, a series of motor positions and data from sensor 7 are recorded. j Related torque data τ mot · The last of these series (consisting of N pairs of measurements and the motor or arm position when these values are obtained) are retained for evaluation. The position can be obtained from the trajectory generator command Preferably, they are obtained from the rotary transformer 8 simultaneously with the torque measurement. j The readings are derived from .
[0047] Optionally, a spectral analysis can be performed on the series in order to find the frequency components that contribute most to the fluctuations in the torque data. Otherwise, it can simply be assumed that components that were found to provide significant contributions in other manipulators will also play an important role here.
[0048] In step S9, the sum of the sinusoidal curves of the M harmonics is Fit to the N data points collected, where b = Θp is solved to obtain the parameter vector p, where
[0049]
[0050] and
[0051]
[0052] where k i (i=1, ..., M) refers to the important frequency components mentioned above. If the most important frequency component is known, for example, the motor 9 j The rotation frequency is twice that of the motor 9j and is 2*i f / i c times, we can set M = 2, k2 = 2 for the second harmonic of the motor rotation and k = 2 for the nearby contribution of the harmonic gear. 1 =2 i f / i c The parameter vector p, hereinafter referred to as the feedback parameter vector, contains the sine and cosine values A for each frequency component S,i ,A C,i (which is equivalent to characterizing the components by amplitude and phase) and a constant offset C that handles the constant bias.
[0053] If the joint is subjected to position-dependent gravity loads (such as Figure 4 Middle joint 5 2 ), the torque data may be high-pass filtered (S8) before being evaluated in step S9. j The transmission ratio is R j, the filter threshold frequency should be between motor 9 j The rotation speed and R j Multiply that frequency by .
[0054] In step S10, the second parameter vector Arbitrary definition is made and transmitted to the motion error calculator 13 j , so that it can output motion error compensation
[0055]
[0056] Among them, when q mot,j is the motor angle output by trajectory generator 11, And θ′ i,j =tan -1 (A′ C,i / A′ S,i ). In step S11, a rotation around the axis 6 is performed. j Another rotation, wherein the motor receives the signal at adder 14 j , 15 j Compensation δq is added ke (q mot,j ),δq ke (q mot，j ) command. The torque data thus generated are collected, and the torque data b is evaluated (S12) in steps S8 and S9 as described above, thereby obtaining The second feedback parameter vector.
[0057] The processing of these data (S12) will be described with reference to FIG6. FIG6 is a graph showing an arbitrary frequency component k of a parameter vector. i Component A S,i , A C,i It should be understood that this two-dimensional representation is chosen only because it is difficult to illustrate more than two dimensions in graphical form; that is, what is shown in the diagrams is actually a two-dimensional projection of a multidimensional vector. The left part of each diagram shows in solid lines the vectors that are being moved by the motion error calculator 13 j The components of the parameter vector used to calculate motion error compensation according to equation (2); while the right hand side shows the components of the parameter vector extracted from the torque data using equation (1).
[0058] Fig. 6A The diagram shows the situation of step S3, in which the motion error calculator 13 j The parameter vector used is 0, but in step S9 a vector p≠0 is derived from the torque data (wherein the component p i ).
[0059] Figure 6BThe components p′ of the second parameter vector p′ are shown i =[A′ S,i ,A′ C,i ] and the components of the second feedback vector p′ generated thereby If there is no potential defect in the first feedback vector p≠0 that produces the parameter vector 0, the second parameter vector p′ can be expected to produce an ideal feedback vector and p′ i The angle β between the two i and gain factor Related,
[0060] in and That is, β i and g i Yes Figure 6B The component p′ i Transform to components The parameters of the transformation. When these parameters are known, we can find the parameter vector, which is transformed into -p( Figure 6C ).
[0061] In step S13, the parameter vector p″ is transmitted to the motion error calculator 13 j , so that it can be used to calculate motion error correction in the future by inserting the components of p″ in equation (2).
[0062] This process is performed for each joint one by one. When then (S14) the controller 2 enters the calculation with all the motion errors 13 j In production mode, the correction of equation 2 is applied, due to the reduction gear 10 j The speed fluctuation of the reference point caused by defects is basically eliminated.
[0063] Reference numerals
[0064] 1 Manipulator
[0065] 2 Controller
[0066] 3 Base
[0067] 4 Connecting rod
[0068] 5 Joints
[0069] 6 Axis
[0070] 7 Accelerometer
[0071] 8. Resolver
[0072] 9 Motor
[0073] 10 Harmonic Drive Gear
[0074] 11. Trajectory Generator
[0075] 12 Joint Controller
[0076] 13 Motion Error Calculator
[0077] 14 Adder
[0078] 15 Adder
[0079] 16 Wave Generator
[0080] 17 Output shaft
[0081] 18 Flexspline
[0082] 19 Steel Wheel
Claims
1. A method for reducing the joint (5) between the distal end portion and the proximal end portion of a manipulator (1) j ) in the method of motion error, the joint (5 j ) has a motor associated therewith (9 j ), the motor (9 j ) is installed in one of the parts and coupled to the other of the parts for being connected to the shaft by a reduction gear (10 j ) driving the distal portion to rotate relative to the proximal portion, the method comprising the following steps: a. Provide a motion correction generator (13) that can be programmed using a parameter vector (0, p', p') j ), and based on the parameter vector and position (q mot,j ) input to output motion correction The motion correction is the sum of at least one sinusoid having: -Frequency(k i ), the frequency (k i ) is the motor (9 j ) or the rotation frequency of the reduction gear (10 j ) at a frequency greater than the frequency of said rotation of said distal portion or an integer multiple thereof, and - Amplitude and phase defined by said parameter vector (0, p', p''); b. Outputting a first drive control signal specifying a motor speed to the motor, wherein the motor speed is a standard speed With the motion correction generator (13 j ) is based on the first motion correction output of the first parameter vector (0) of and; c. from the joint (5) in response to the first drive control signal j ) receives an evaluation signal (τ) and extracts the frequency (k) of the at least one sinusoid i ) and determining a first feedback amplitude (A) defining said frequency component i,j ) and the first feedback phase The first feedback vector (p); d. Specify the motor speed The second drive control signal is output to the motor (9 j ), the motor speed is the standard speed With the motion correction generator (13 j ) A second motion correction based on the output of the second parameter vector (p') of and; e. from the joint (5) in response to the second drive control signal j ) receives an evaluation signal (τ) and extracts the frequency (k) of the at least one sinusoid i ), and determining a second feedback amplitude defining the frequency component and the second feedback phase The second feedback vector f. From the second feedback vector Subtract the first feedback vector (p) to obtain the feedback difference vector g. Find the feedback difference vector a transformation that transforms into the difference (p'-0) between the second parameter vector and the first parameter vector; h. Applying the transformation to the second feedback vector To obtain the third parameter vector (p”); i. Using the third parameter vector (p") to the motion correction generator (13 j ) for programming.
2. The method of claim 1, wherein the amplitude defined by the first parameter vector (0) is zero.
3. The method according to claim 1 or 2, wherein the standard speed is a constant speed.
4. The method according to any one of the preceding claims, wherein the step of extracting the frequency component having the frequency of the at least one sinusoid comprises: The evaluation signal (τ) is subjected to high-pass filtering (S8), wherein the cut-off frequency is optionally lower than the motor frequency and higher than the reduction ratio (R j ) are multiplied.
5. A method according to any one of the preceding claims, wherein the standard speed or its harmonics corresponding to the resonant frequency of the manipulator (1).
6. The method according to claim 5, further comprising at least one of the following steps: - controlling (S3) the distal end portion to rotate at various speeds, and selecting (S5) a speed at which the vibration of the manipulator (1) is strongest among the various speeds as the standard speed - controlling (S1) said distal portion to adopt a configuration maximizing its moment of inertia. 7 . The method according to claim 1 , wherein the evaluation signal (τ) represents the motor torque.
8. The method according to any one of the preceding claims, wherein the reduction gear (10 j ) is a c A toothed wheel (19) and a f The harmonic gear of the flexible wheel (18) has 2 teeth, and the frequency of the at least one sinusoidal curve is the frequency of the motor (9 j ) of the rotation frequency i f / i c times or its integer multiples.
9. The method of claim 8, wherein the integer multiple is 2i of the rotation frequency. f / i c times.
10. A method according to any one of the preceding claims, wherein the motion correction is the sum of two or more sinusoids.
11. The method according to any one of the preceding claims, wherein the motion correction generator (13 j ) steps include: A processor is programmed to operate as the motion correction generator.
12. A robot system comprising: - a manipulator (1) having a proximal portion, a distal portion, a joint (5) connecting the proximal portion and the distal portion j ) and for driving the joint (5 j )Rotating motor (9 j ), -Controller(12 j ), connected to the motor (9 j ) and is adapted to perform at least steps b to i of the method according to any one of claims 1 to 11.
13. A computer software product comprising a plurality of machine readable instructions which, when executed by a processor (13), cause the processor to perform at least steps b to i of the method according to any one of claims 1 to 11.