Robot pose synchronization speed planning method with time rounding function

Abstract

The invention provides a robot pose synchronization speed planning method with time rounding, and relates to the field of robot trajectory speed planning interpolation. The method comprises the following steps: reading a pose point location to be planned, and converting the pose of the robot to quaternion logarithm for expression; constructing a corner curve in the quaternion logarithm space by adopting a finite second dichotomy; on the basis of the constructed corner curve, the corner speed of the corner curve is restrained; and based on the corner curve after the corner speed is restrained, speed synchronization of the pose curve and the attitude curve is carried out. According to the method, the speed of the robot at the corner is prevented from being excessively reduced through a backtracking method, so that the robot has a high speed, and the machining efficiency can be effectively improved; in addition, the method can effectively achieve smooth planning of poses and postures, and then the smoothness of joint movement of the robot is guaranteed.

Description

Technical Field

[0001] The invention relates to the field of robot trajectory speed planning and interpolation, and in particular to a robot posture synchronous speed planning method with time rounding. Background Art

[0002] Industrial robots are widely used in industrial production due to their good flexibility. The traditional industrial robot trajectory interpolation widely uses the posture synchronization method. However, the traditional curve parameter synchronization method cannot simultaneously constrain the kinematic parameters of the position movement and posture movement of the industrial robot, resulting in discontinuous speed of the posture movement at the corner of the line segment. Moreover, the total interpolation time of the robot is inevitably not an integer multiple of the interpolation period during the interpolation process, which easily causes vibration during the robot processing process.

[0003] There is a posture synchronization method based on time synchronization in the prior art, but this method is achieved by suppressing the corner velocity, which makes it difficult to achieve efficient interpolation, and it does not consider the impact of interpolation time rounding on the planning process.

[0004] The Chinese invention patent with publication number CN118276514A discloses a corner smoothing velocity planning method based on a time rounding strategy, which can effectively round off the interpolation time, but does not consider the posture synchronization of the robot.

[0005] Therefore, a speed planning scheme is needed to improve the smoothness of robot joint operation in order to enhance processing quality and efficiency. Summary of the invention

[0006] Purpose of the invention: To propose a robot posture synchronization speed planning method with time rounding to improve the joint vibration of the robot during operation, improve the processing quality and efficiency, and thus solve the above-mentioned problems existing in the prior art.

[0007] The present invention proposes a robot posture synchronization speed planning method with time rounding, comprising the following steps:

[0008] Read the posture points to be planned and convert the robot's posture into quaternion logarithmic space for expression;

[0009] Constructing a corner curve by using a finite-order dichotomy method in the quaternion logarithmic space;

[0010] Based on the constructed corner curve, constraining the corner speed;

[0011] Based on the corner curve after constraining the corner velocity, the speed synchronization planning of the posture curve and the position curve is carried out.

[0012] In a further embodiment, the specific process of converting the robot's posture into a quaternion logarithmic space for expression includes:

[0013] The robot rotates around the Z, Y, and X axes of the coordinate system respectively , , , the attitude angle The corresponding quaternion q satisfies the following formula:

[0014]

[0015] In the formula, Represents quaternion multiplication; , , , They are the four elements of the quaternion respectively; A quaternion representing rotation around the Z axis; Quaternion representing rotation around the Y axis; A quaternion representing rotation around the X axis;

[0016] in:

[0017] Quaternion rotation around the Z axis ;

[0018] Quaternion rotation around the Y axis ;

[0019] Quaternion rotation around the X axis ;

[0020] Quaternion Logarithm As follows:

[0021]

[0022] In the formula, , , is the corresponding quaternion logarithmic space expression; n represents the axis of rotation, and , ;

[0023] in:

[0024]

[0025] In the formula, , , , They are the four elements of the quaternion.

[0026] In a further embodiment, a finite-order dichotomy method is used in the quaternion logarithmic space to construct a corner curve, specifically comprising:

[0027] In the quaternion logarithmic space, a finite number of bisection methods are used to approximate the corner curve, and the midpoint of the corner curve is calculated based on half of the shortest line segment where the corner is located. The midpoint of the corner curve and its corresponding vertex are converted to the quaternion space, and the attitude error between the two points is calculated. After a finite number of iterations, the optimal corner curve is obtained.

[0028] In a further embodiment, when calculating the posture error between two points, if the preset error condition is met for the first time, the iteration process is exited, and the corner curve at this time is the optimal corner curve.

[0029] In a further embodiment, based on the constructed corner curve, different speed limits are applied to the positions of the corners, as shown in the following formula:

[0030]

[0031] In the formula, is the normal velocity of the corner curve; It is the maximum speed that can be achieved when the initial velocity and displacement are specified; is the maximum speed limit; The synchronous constraint speed of the line segment where the last corner is located.

[0032] In a further embodiment, based on the corner curve after constraining the corner velocity, speed synchronization planning of the posture curve and the position curve is performed, specifically including:

[0033] Perform independent speed planning for position motion and attitude motion, obtain the minimum position segment where the speed changes for both, and obtain the kinematic information at the corresponding position;

[0034] The position n to be synchronized and the synchronization time Satisfy the following formula:

[0035]

[0036] In the formula, Indicates the minimum position segment where the speed of position movement changes; Indicates the minimum position segment where the speed of posture movement changes; , Respectively represent the motion time of the nth segment position motion and posture motion;

[0037] To synchronize time Based on the time, the speed and acceleration of the position and posture motion at each corresponding position are adjusted to make the motion time of each position curve and posture curve adjusted to the synchronous time. ; If the adjusted initial velocity has not changed, the position to be synchronized is updated to synchronize the next posture; if the adjusted initial velocity has changed, the velocity of the corresponding corner is updated and the synchronization segment is traced back to the previous segment to resynchronize the position and posture.

[0038] In a further embodiment, when the movement time of the position movement or posture movement is 0, the total time of the curve with the movement time of 0 is adjusted to the movement time of another curve at the corresponding position, and the speed and acceleration are no longer adjusted.

[0039] In a further embodiment, independent speed planning is performed for position and posture respectively. During forward planning, the speed is calculated based on a calculation formula with time rounding of an S-shaped speed curve. Only one line segment is planned each time. If the initial speed of the line segment does not change, synchronous planning is performed.

[0040] If the initial velocity of the line segment changes, trace back to the segment where the initial velocity of the line segment does not change.

[0041] Compared with the prior art, the present invention has the following significant advantages:

[0042] (1) This method avoids the robot's speed being excessively reduced at the corner by backtracking, so that the robot has a higher speed, which can effectively improve the processing efficiency;

[0043] (2) The posture synchronization method proposed in this paper can effectively realize the smooth planning of posture and attitude, thereby ensuring the smoothness of the robot's joint motion;

[0044] (3) This method modifies the existing time rounding method to adapt it to robot planning and thus effectively improve the vibration during robot operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is the flow chart of posture synchronization speed planning.

[0046] Figure 2 This is a schematic diagram of synchronous speed planning.

[0047] Figure 3 It is a schematic diagram of the time synchronization strategy.

[0048] Figure 4 is the original pose path graph.

[0049] Figure 5 is the maximum error map at the corner.

[0050] Figure 6 It is a speed curve diagram of the comparison method.

[0051] Figure 7 It is the speed curve of the proposed method. DETAILED DESCRIPTION

[0052] In the following description, a large number of specific details are provided to provide a more thorough understanding of the present invention. However, it is apparent to those skilled in the art that the present invention can be implemented without one or more of these details. In other examples, in order to avoid confusion with the present invention, some technical features known in the art are not described.

[0053] This embodiment discloses a specific process of a robot posture synchronization speed planning method with time rounding, and the flow chart is as follows: Figure 1 As shown, the following steps are included:

[0054] Step 1: Read the position points to be planned and convert the robot's posture into quaternion logarithmic space for expression. Convert the robot's posture expressed by ZYX Euler angles into quaternion logarithmic expression as follows: Assume that the rotations around the Z, Y, and X axes of the coordinate system are , , , we can get the attitude angle The corresponding quaternion q satisfies the following formula (1):

[0055]

[0056] in, represents quaternion multiplication. q can be written as the following formula (2):

[0057]

[0058] in:

[0059]

[0060] The quaternion logarithm is expressed as follows (3):

[0061]

[0062] in, is the corresponding quaternion logarithmic space expression.

[0063] Step 2: Construct the corner curve based on the limited error. Based on the given form of the corner curve, the position is constructed analytically, but the attitude curve is constructed in the quaternion logarithmic space using a finite number of bisection approximation. In the quaternion logarithmic space, when the attitude curve is constructed using a finite number of bisection approximation, the midpoint of the corner curve is calculated based on half of the shortest line segment where the corner is located. The midpoint of the corner curve and its corresponding vertex are converted to the quaternion space, and the attitude error between the two points is calculated. Through a finite number of iterations, the optimal corner curve is achieved. When calculating the error between two points, if the error condition is met for the first time, the iterative process is exited, and the corner curve at this time is the optimal corner curve.

[0064] Step 3: Limit the corner speed constraint of the planning segment. In a planning segment, different speed constraints are applied to the corners, as shown in the following formula (4):

[0065]

[0066] in, is the normal velocity of the corner curve. It is the maximum velocity that can be achieved when the initial velocity and displacement are specified. is the maximum speed limit. is the synchronous constraint speed of the line segment where the last corner is located, and its magnitude is: The maximum speed that can be accelerated from 0 in the reverse direction under the condition of multiple.

[0067] Step 4: Perform synchronous speed planning for the posture segment from front to back. Perform independent speed planning for the position and posture respectively, and the speed curve is an S-shaped speed curve. Obtain the minimum position segment where the speed changes for both, and obtain the kinematic information at the corresponding position, such as Figure 2 As shown. Then the position to be synchronized n and the synchronization time Satisfies the following formula (5):

[0068]

[0069] In the formula, Indicates the minimum position segment where the speed of position movement changes; Indicates the minimum position segment where the speed of posture movement changes; , They represent the motion time of the nth segment of position motion and posture motion respectively.

[0070] Synchronize the synchronization time and the kinematic information of the trajectory to be synchronized. If the initial velocity output by the time synchronization module has not changed, update the position to be synchronized to synchronize the next segment of posture. If the initial velocity output changes, update the velocity of the corresponding corner and trace the synchronization segment back to the previous segment to resynchronize the position and posture. When the motion time of any trajectory is 0, only the longest time needs to be updated, and there is no need to synchronize the velocity and acceleration.

[0071] Combination Figure 3 The calculation of the time synchronization module is explained as follows: In the macro acceleration state where ACC, CRU, and DEC generally exist at the same time ( ) as an example (represented by A0), the total motion time is T. The synchronous motion time is ,and .set up is the maximum speed, is the maximum acceleration, is the maximum acceleration, is the ACC stage time of the original speed curve, is the DEC phase time of the original speed curve, It is the displacement of the ACC stage of the original velocity curve. Figure 2 Six synchronization types are shown, which can synchronize the time of a motion trajectory to any time.

[0072] Step 1: Determine according to the following formula (6): The critical motion time of the type curve. , jump to step 2; otherwise, go to step 3.

[0073]

[0074] Step 2: Use the A1 curve to synchronize the A0 curve, and keep the motion time of the ACC and DEC stages of the original speed curve. to To achieve time synchronization. Correction of maximum speed Satisfies the following formula (7):

[0075]

[0076] Since the synchronized curve does not need to maintain the same motion type as the original speed curve, the jerk in the ACC and DEC stages can be obtained by simple calculation. Taking the acceleration in the ACC stage as an example, assuming that there is only a variable acceleration stage, the jerk accJ satisfies the following formula (8):

[0077]

[0078] like , it means that there is a uniform acceleration stage in the acceleration stage, so adjust , and calculate the time of each stage according to the following formula (9). The calculation of the acceleration in the deceleration stage is similar.

[0079]

[0080] in, , It is the time of the first and second stages of the acceleration phase.

[0081] Step 3: Calculate the critical duration of the T2 curve according to the following formula (10). If , jump to step 4; otherwise, go to step 5;

[0082]

[0083] Step 4: Use the A2 curve to synchronize the A0 curve. The time of the ACC stage is determined by Extend to .time The following formula (11) is satisfied.

[0084]

[0085] Step 5: Calculation Time to decelerate to 0 and displacement .if , then synchronize the A3 curve, otherwise go to step 6. Let the movement time of the two-stage variable speed movement remain , , and calculate .like , then the time of the first stage Adjust to the following formula (12) and recalculate .

[0086]

[0087] Step 6: Calculate the critical duration of the T3 curve according to the following formula (13). If , jump to step 7; otherwise, go to step 8;

[0088]

[0089] Step 7: Use A4 curve to synchronize A0 curve. At this time, the ACC stage time Extends to The maximum speed at this time The following formula (14) is satisfied.

[0090]

[0091] Step 8: Calculation Time to decelerate to 0 and displacement .if , then synchronize the A5 curve, otherwise go to step 9. Let the duration of the two-stage variable speed motion be , calculated according to the above formula .

[0092] Step 9: Use the A6 curve to synchronize the A0 curve. At this time, it is a uniform motion. Satisfies the following formula (15):

[0093]

[0094] When the position and attitude trajectories are planned independently, the speed is calculated based on the calculation formula of the S-shaped speed curve with time rounding during forward planning, referring to the calculation formula proposed in the Chinese invention patent "A corner smoothing speed planning method based on time rounding strategy" with publication number CN118276514A. Only one line segment is planned each time. If the initial speed of the line segment does not change, synchronous planning is performed; if the initial speed of the line segment changes, it is traced back to the segment where the initial speed of the line segment does not change.

[0095] Embodiment effect:

[0096] The comparison method (Real-time feedrate scheduling for five-axis machining by simultaneously planning linear and angular trajectories, Jie Huang, Yaoan Lu, Li-Min Zhu, International Journal of Machine Tools and Manufacture, pp. 78-96, published online on 2018-09-05) has the same corner curve as the present application, but without the time rounding strategy. Set the position speed to meet , acceleration and jerk satisfy , The constraints of the posture motion are satisfied , , The linear path and attitude errors satisfy , The interpolation period is 2ms. The path pose to be planned is as follows: Figure 4 shown.

[0097] The simulation effect is as follows Figures 5 to 7 shown. Figure 5 In the figure, (a) is the position error diagram, and (b) is the attitude error diagram; Figure 6 In the figure, (a) is the position velocity diagram, and (b) is the attitude velocity diagram; Figure 7 In the figure, (a) is the position velocity diagram, and (b) is the attitude velocity diagram. Figure 5 It can be seen that the maximum errors of the tool position and posture at the corner are limited within the constraint range, which verifies that the proposed corner construction method is error controllable. Figure 6 and Figure 7 It can be seen that both methods can effectively realize speed constraint and synchronization. In a simple path, the comparison method takes 1164ms, while the method in this paper takes 1066ms, and the interpolation efficiency is improved by 8.42%; when the path environment is more complex, the present invention can achieve higher efficiency. Figure 7 As can be seen in (b), the proposed method can achieve time rounding by reducing the maximum speed, thereby effectively eliminating the end vibration caused by speed fluctuations caused by time rounding errors, but the comparison method does not have a time rounding strategy. Under the premise of ensuring time rounding, the interpolation efficiency of the proposed method is still higher than that of the comparison method, indicating that the posture synchronization algorithm of the proposed method has better performance and can achieve higher processing quality and efficiency in processing.

[0098] The technical process of the robot posture synchronization speed planning method disclosed in the above embodiment can be implemented in whole or in part through software, hardware, firmware or any other combination.

[0099] When implemented using hardware, the above embodiments can run all or part of the working logic and computing process on an electronic device after being compiled by software. The electronic device includes a processor, a memory, a communication interface, and a communication bus. The processor, the memory, and the communication interface communicate with each other via the communication bus. The memory is used to store at least one executable instruction, and the executable instruction enables the processor to execute the technical process disclosed in the above embodiments.

[0100] When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. If the above method is implemented in the form of a software function module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present application can be essentially or partly embodied in the form of a software product that contributes to the relevant technology. The software product is stored in a storage medium, including several instructions to enable an electronic device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the methods described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a U disk, a mobile hard disk, a read-only memory (ROM), a disk or an optical disk. In this way, the embodiment of the present application is not limited to any specific hardware, software or firmware, or any combination of hardware, software, and firmware.

[0101] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0102] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A robot posture synchronization speed planning method with time rounding, characterized in that: The steps include: Read the position points to be planned and convert the robot's posture into quaternion logarithmic space for expression; Constructing a corner curve by using a finite-order dichotomy method in the quaternion logarithmic space; Based on the constructed corner curve, constraining the corner speed; Based on the corner curve after constraining the corner velocity, the speed synchronization planning of the posture curve and the position curve is carried out.

2. The robot posture synchronization speed planning method with time rounding according to claim 1 is characterized in that: Read the position points to be planned. The rotation of the robot around the Z, Y, and X axes of the coordinate system are , , , the attitude angle The corresponding quaternion q satisfies the following formula: ； In the formula, Represents quaternion multiplication; , , , They are the four elements of the quaternion respectively; A quaternion representing rotation around the Z axis; Quaternion representing rotation around the Y axis; A quaternion representing a rotation around the X axis.

3. The robot posture synchronization speed planning method with time rounding according to claim 2 is characterized in that: Quaternion rotation around the Z axis The expression is as follows: ； Quaternion rotation around the Y axis The expression is as follows: ； Quaternion rotation around the X axis The expression is as follows: ； In the formula, , , They are the rotation angles of the robot around the Z, Y, and X axes of the coordinate system.

4. The robot posture synchronization speed planning method with time rounding according to claim 3 is characterized in that: The robot's posture is converted into quaternion logarithms for expression. As follows: ； In the formula, , , is the corresponding quaternion logarithmic space expression; n represents the quaternion rotation axis, and , ; in: ； In the formula, , , , They are the four elements of the quaternion.

5. The robot posture synchronization speed planning method with time rounding according to claim 1 is characterized in that: A finite-order dichotomy method is used in the quaternion logarithmic space to construct a corner curve, specifically including: In the quaternion logarithmic space, a finite number of bisection methods are used to approximate the corner curve, and the midpoint of the corner curve is calculated based on half of the shortest line segment where the corner is located. The midpoint of the corner curve and its corresponding vertex are converted to the quaternion space, and the attitude error between the two points is calculated. After a finite number of iterations, the optimal corner curve is obtained.

6. The robot posture synchronization speed planning method with time rounding according to claim 5 is characterized in that: When calculating the posture error between two points, if the preset error condition is met for the first time, the iteration process is exited and the corner curve at this time is the optimal corner curve.

7. The robot posture synchronization speed planning method with time rounding according to claim 1 is characterized in that: Based on the constructed corner curve, different speed limits are applied to the corner positions, as shown in the following formula: ； In the formula, is the normal velocity of the corner curve; It is the maximum velocity that can be achieved when the initial velocity and displacement are specified; is the maximum speed limit; The synchronous constraint speed of the line segment where the last corner is located.

8. The robot posture synchronization speed planning method with time rounding according to claim 1 is characterized in that: Based on the corner curve after constraining the corner velocity, the speed synchronization planning of the attitude curve and the position curve is carried out, including: Perform independent speed planning for position motion and attitude motion, obtain the minimum position segment where the speed changes for both, and obtain the kinematic information at the corresponding position; The position n to be synchronized and the synchronization time Satisfy the following formula: ； In the formula, Indicates the minimum position segment where the speed of position movement changes; Indicates the minimum position segment where the speed of posture movement changes; , Respectively represent the motion time of the nth segment position motion and posture motion; To synchronize time Based on the time, the speed and acceleration of the position and posture motion at each corresponding position are adjusted to make the motion time of each position curve and posture curve adjusted to the synchronous time. ; If the adjusted initial velocity has not changed, the position to be synchronized is updated to synchronize the next posture; if the adjusted initial velocity has changed, the velocity of the corresponding corner is updated and the synchronization segment is traced back to the previous segment to resynchronize the position and posture.

9. The robot posture synchronization speed planning method with time rounding according to claim 8, characterized in that: When the motion time of the position motion or the posture motion is 0, the total time of the curve with the motion time of 0 is adjusted to the motion time of another curve at the corresponding position, and the speed and acceleration are no longer adjusted.

10. The robot posture synchronization speed planning method with time rounding according to claim 8, characterized in that: Independent speed planning is performed for position and posture respectively. During forward planning, the speed is calculated based on the calculation formula of the S-shaped speed curve with time rounding. Only one line segment is planned at a time. If the initial speed of the line segment does not change, synchronous planning is performed. If the initial velocity of the line segment changes, trace back to the segment where the initial velocity of the line segment does not change.

Images