A method for planning gantry-like trajectories of articulated decoupled robots

Abstract

This invention provides a method for planning gantry-like trajectories of articulated decoupled robots, comprising: decomposing the gantry-like motion trajectory; horizontal and vertical motion joints; calculating the travel distance and rotation angle; calculating the travel time based on preset speed, acceleration, and jerk; iteratively finding the optimal speed, acceleration, and jerk for path planning; obtaining the interpolation value for each interpolation cycle on the path; and interpolating the path according to each interpolation value for each interpolation cycle. This invention does not require the speed to drop to zero at connection points, and the rotary joints and linear motion joints have their own corresponding speed parameters, thus better utilizing the robot's motion performance.

Description

Technical Field

[0001] This invention relates to the field of posture control technology for transport robots, and more specifically, to a method for planning gantry-like trajectories for articulated decoupled robots. Background Technology

[0002] In production applications, automated production lines not only enhance a company's competitiveness and responsiveness but also promote the establishment and development of new industries and drive technological advancements in related disciplines, demonstrating significant social and economic benefits. However, due to the demands of the working environment and equipment, it is inevitable to improve the cycle time of robot handling to maximize robot performance and meet the demands of modern, efficient production. Gantry trajectories, being the most commonly used trajectory in most industrial scenarios, allow for the transport of more objects within a given timeframe with shorter movement times, effectively improving production efficiency.

[0003] Traditional portal-like trajectory planning methods mainly involve stitching together a portal-like trajectory at four locations in space. These locations can be connected by straight lines or curves, the difference being the planning space. Generally, these are divided into Cartesian space planning and joint space planning. Both methods can achieve portal-like trajectories. This type of method is simple and intuitive to program. However, because the direction changes at the connection points, the speed needs to be reduced to zero at those points before restarting, which affects efficiency. A common solution is to use circular arcs at the connection points for transition, eliminating the need to reduce the speed to zero and restart. However, this method is theoretically complex and computationally time-consuming. For certain robots, such as SCARA robots, since the joints of SCARA robots mainly consist of rotary joints for horizontal movement and linear joints for vertical movement, the different dimensions of motion between the rotary and linear joints during movement can limit the robot's performance. Summary of the Invention

[0004] The problem solved by this invention is to propose a gantry-like trajectory planning method for articulated decoupled robots, which does not require the velocity to drop to zero at the connection point, and the rotary joints and linear joints have their own corresponding velocity parameters, thereby better utilizing the robot's motion performance.

[0005] To address the above problems, this invention provides a method for planning gantry-like trajectories for articulated decoupled robots, comprising the following steps:

[0006] S1. Decompose the gantry-type motion trajectory from the starting point to the ending point into a horizontal trajectory and a vertical trajectory;

[0007] S2. Decouple the robot's motion joints according to the horizontal and vertical trajectories to obtain the horizontal motion joints corresponding to the horizontal trajectory and the vertical motion joints corresponding to the vertical trajectory.

[0008] S3, calculate the distance the horizontal joint needs to move; S1, the distance the vertical joint needs to lift; S2, the distance the vertical joint needs to lower; S3, and the angle Δq that each axis on the horizontal joint needs to rotate. i Let i represent the i-th axis of rotation on the horizontal motion joint; the required rotational length L for each axis of rotation on the horizontal motion joint is:

[0009]

[0010] n is the total number of axes of rotation on the joints moving in the horizontal direction;

[0011] S4. Calculate the horizontal movement time T1 of the joint at distance S1 based on the preset velocity v1, acceleration a1, and jerk j1. Calculate the vertical movement time T2 at distance S2 and the vertical movement time T3 at distance S3 based on the preset velocity v2, acceleration a2, and jerk j2.

[0012] S5. Determine the relationship between T1, T2, and T3. If T1 > T2 + T3, proceed to step S9. If T1 ≤ T2 + T3, proceed to step S6.

[0013] S6, within the range of velocity v2 [V 2min V 2max If a new velocity v2 is found within the range [V] such that T1 > T2 + T3, proceed to step S9. If the range of velocity v2 is within the range [V], then... 2min V 2max If all values ​​in the range make T1≤T2+T3, then proceed to step S7;

[0014] S7, the range of a fixed velocity v2 [V 2min V 2max The final velocity v2 found within the range of acceleration a2 remains unchanged. 2min ,a 2max and the range of jerk j2 [j 2min ,j 2max Within the range [a], find new accelerations a2 and j2 such that T1 > T2 + T3. If new accelerations a2 and j2 are found such that T1 > T2 + T3, proceed to step S9. If the range of acceleration a2 is [a], ... 2min ,a 2maxand the range of jerk j2 [j 2min ,j 2max If all values ​​in the range make T1≤T2+T3, then proceed to step S8;

[0015] S8. Fix the interval range of v2 respectively [V 2min V 2max ], the range of acceleration a2 [a 2min ,a 2max and the range of jerk j2 [j 2min ,j 2max The final velocity v2, acceleration a2, and jerk j2 found within the range remain unchanged; a new velocity v1 is found in the range [0, v1] such that T1 = Thd(T2 + T3), where Thd is the compensation value to ensure synchronization of the horizontal and vertical joints, and Thd > 1;

[0016] S9. Based on the velocity v1, acceleration a1, jerk j1, velocity v2, acceleration a2, and jerk j2 satisfying T1>T2+T3 or T1=Thd(T2+T3), plan a path and obtain the interpolation value ΔS for each interpolation cycle in the path. k , k represents the kth interpolation period;

[0017] S10. Based on each interpolation value ΔS k We obtain the mapping functions q(i) for each axis on the horizontal and vertical motion joints:

[0018]

[0019] And based on each interpolation value ΔS k The path is interpolated according to each interpolation cycle.

[0020] In this method, the horizontal motion joints are rotary joints, and the vertical motion joints are linear motion joints. Both the horizontal and vertical motion joints have their own corresponding velocity parameters, and the corresponding velocity parameters are all optimal values ​​found through iterative calculations. It is not necessary for the velocity to drop to zero at the connection point, so as to better utilize the robot's motion performance.

[0021] Furthermore, in steps S6, S7, and S8, new velocities v2, accelerations a2, jerk j2, and velocities v1 are found using the bisection method.

[0022] This setup reduces computational power, allows for faster iterative calculations, and thus improves the continuity of the robot's movements.

[0023] Furthermore, in step S9, the path is a seven-segment S-shaped curve, where the displacement function d(t) is:

[0024]

[0025]

[0026] Where v0 represents the initial velocity, j represents the jerk, a represents the acceleration, v represents the maximum velocity, T1 to T7 represent the motion time of each segment, t represents any motion time within the total motion time from T1 to T7, and s represents any length within the total length of the curve;

[0027] The velocity function v(t) is:

[0028]

[0029] In this setup, the seven-segment S-curve divides the entire gantry-like motion trajectory from the starting point to the end point into seven stages. The acceleration is continuous at the speed transitions between each stage, avoiding soft impacts during robot movement and resulting in a smooth motion process. The seven-segment S-curve can also be adjusted according to different path lengths and specific robot parameters to adapt to different application scenarios and needs, making it highly adaptable. Attached Figure Description

[0030] Figure 1 This is a schematic diagram of the steps of the present invention; Detailed Implementation

[0031] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below.

[0032] This embodiment provides a method for planning gantry-like trajectories for articulated decoupled robots, including the following steps:

[0033] S1. Decompose the phylogenetic motion trajectory from the starting point to the ending point into a horizontal trajectory and a vertical trajectory; S2. Decouple the robot's motion joints according to the horizontal and vertical trajectories to obtain the horizontal motion joints corresponding to the horizontal trajectory and the vertical motion joints corresponding to the vertical trajectory;

[0034] S3, calculate the distance the horizontal joint needs to move; S1, the distance the vertical joint needs to lift; S2, the distance the vertical joint needs to lower; S3, and the angle Δq that each axis on the horizontal joint needs to rotate. i Let i represent the i-th axis of rotation on the horizontal motion joint; the required rotational length L for each axis of rotation on the horizontal motion joint is:

[0035]

[0036] n is the total number of axes of rotation on the joints moving in the horizontal direction;

[0037] S4. Calculate the horizontal movement time T1 of the joint at distance S1 based on the preset velocity v1, acceleration a1, and jerk j1. Calculate the vertical movement time T2 at distance S2 and the vertical movement time T3 at distance S3 based on the preset velocity v2, acceleration a2, and jerk j2.

[0038] S5. Determine the relationship between T1, T2, and T3. If T1 > T2 + T3, proceed to step S9. If T1 ≤ T2 + T3, proceed to step S6.

[0039] S6, within the range of velocity v2 [V 2min V 2max If a new velocity v2 is found within the range [V] such that T1 > T2 + T3, proceed to step S9. If the range of velocity v2 is within the range [V], then... 2min V 2max If all values ​​in the range make T1≤T2+T3, then proceed to step S7;

[0040] S7, the range of a fixed velocity v2 [V 2min V 2max The final velocity v2 found within the range of acceleration a2 remains unchanged. 2min ,a 2max and the range of jerk j2 [j 2min ,j 2max Within the range [a], find new accelerations a2 and j2 such that T1 > T2 + T3. If new accelerations a2 and j2 are found such that T1 > T2 + T3, proceed to step S9. If the range of acceleration a2 is [a], ... 2min ,a 2max and the range of jerk j2 [j 2min ,j 2max If all values ​​in the range make T1≤T2+T3, then proceed to step S8;

[0041] S8. Fix the interval range of v2 respectively [V 2min V 2max ], the range of acceleration a2 [a 2min ,a 2max and the range of jerk j2 [j 2min ,j 2maxThe final velocity v2, acceleration a2, and jerk j2 found within the range remain unchanged; a new velocity v1 is found in the range [0, v1] such that T1 = Thd(T2 + T3), where Thd is the compensation value to ensure synchronization of the horizontal and vertical joints, and Thd > 1;

[0042] S9. Based on the velocity v1, acceleration a1, jerk j1, velocity v2, acceleration a2, and jerk j2 satisfying T1>T2+T3 or T1=Thd(T2+T3), plan a path and obtain the interpolation value ΔS for each interpolation cycle in the path. k , k represents the kth interpolation period;

[0043] S10. Based on each interpolation value ΔS k We obtain the mapping functions q(i) for each axis on the horizontal and vertical motion joints:

[0044]

[0045] And based on each interpolation value ΔS k The path is interpolated according to each interpolation cycle.

[0046] Furthermore, in steps S6, S7, and S8, the new velocity v2, acceleration a2, jerk j2, and velocity v1 are all found using the bisection method.

[0047] Furthermore, in step S9, the path is a seven-segment S-shaped curve, where the displacement function d(t) is:

[0048]

[0049]

[0050] Where v0 represents the initial velocity, j represents the jerk, a represents the acceleration, v represents the maximum velocity, T1 to T7 represent the motion time of each segment, t represents any motion time within the total motion time from T1 to T7, and s represents any length within the total length of the curve;

[0051] The velocity function v(t) is:

[0052]

[0053] Specifically, in this embodiment, a SCARA robot can be used. The starting point of the gantry-type motion trajectory can be defined as point A, and the ending point as point B. Since the movement from point A to point B is a horizontal movement in a two-dimensional plane, the movement trajectory from point A to point B is called the horizontal trajectory. Vertical movements of lifting and lowering are required at both points A and B for picking up or placing items, so the vertical movement trajectory from point A to point B is called the vertical trajectory. During decoupling, the horizontal trajectory corresponds to the horizontal motion joints of the SCARA robot, and the vertical trajectory corresponds to the vertical motion joints of the SCARA robot. Since the horizontal motion joints need to move horizontally in a two-dimensional plane, that is, they need to rotate, the horizontal motion joints are also the rotary joints of the SCARA robot, and the vertical motion joints are also the linear joints of the SCARA robot. It should be noted that the method of the present invention can also be applied to three-axis robots and four-axis robots. Similar to SCARA robots, the only difference between three-axis and four-axis robots is the number of axes of rotation on the horizontal motion joints. It is only necessary to calculate the required rotation angle Δq of each axis in step S3. i The required rotational length L is sufficient, so this embodiment only describes the application on the SCARA robot in detail, and the rest will not be elaborated.

[0054] In step S3, distance S1 is the distance the horizontal motion joint of the SCARA robot moves from point A to point B; distance S2 is the distance the vertical motion joint of the SCARA robot needs to move up at point A or point B; and distance S3 is the distance the vertical motion joint of the SCARA robot needs to move down at point A or point B. Since the horizontal motion joint of the SCARA robot consists of multiple axes, when the horizontal motion joint moves, the rotation angle Δq of the corresponding axis needs to be calculated based on distances S1, S2, and S3. i This is done in conjunction with the value of the rotational module length L; in step S4, the velocity v1, acceleration a1, jerk j1, velocity v2, acceleration a2, and jerk j2 all need to be preset by the user within a suitable range. This range can be flexibly adjusted by those skilled in the art according to actual needs, with the aim of enabling the SCARA robot to complete the movement within a specified time. For example, the range of velocity v2 is [V...]. 2min V 2maxThe rest are similar. In step S4, within a preset range of velocity v1, acceleration a1, jerk j1, velocity v2, acceleration a2, and jerk j2, an optimal velocity v1, acceleration a1, jerk j1, velocity v2, acceleration a2, and jerk j2 are arbitrarily selected as initial values ​​for calculating T1, T2, and T3. If the relationship T1 > T2 + T3 is not satisfied, it indicates that the initial values ​​of velocity v1, velocity v2, acceleration a2, and jerk j2 are not suitable, which will cause the SCARA robot's horizontal and vertical trajectories to be out of sync. Therefore, step S6 is needed to adjust the initial values ​​within the range of velocity v2 [V]. 2min V 2max The binary search method is used iteratively to find the optimal velocity v2 such that T1 > T2 + T3. If the range of velocity v2 is [V 2min V 2max If there is no optimal velocity v2 within the range of acceleration a2 such that T1 > T2 + T3, then the velocity v2 found in the most recent iteration remains unchanged. Then, proceed through step S7 within the range of acceleration a2 [a 2min ,a 2max The binary search method is used iteratively to find the optimal acceleration a2 such that T1 > T2 + T3. If the range of acceleration a2 is [a 2min ,a 2max If there is no optimal acceleration a2 within the range [j] such that T1 > T2 + T3, then keep the velocity v2 and acceleration a2 found in the most recent iteration unchanged, and apply acceleration j2 within the range [j] 2min ,j 2max The binary search method is used to iteratively find the optimal jerk j2 such that T1 > T2 + T3. If the range of jerk j2 is [j 2min ,j 2max If no optimal jerk j2 exists within the range [0, v1] such that T1 > T2 + T3, then the velocity v2, acceleration a2, and jerk j2 found in the most recent iteration remain unchanged. Within the range [0, v1], a new velocity v1 is iteratively searched using a bisection method. This is equivalent to no longer searching for velocity parameters that can change time T2 and time T3. The closer velocity v1 is to 0, the larger T1 becomes, and a velocity v1 satisfying T1 > T2 + T3 can inevitably be found. Therefore, it is not necessary to search for values ​​for acceleration a1 and jerk j1 again. The compensation value Thd can be flexibly adjusted according to the actual situation.

[0055] Once the velocities v1, v2, accelerations a1, a2, jerk j1, and jerk j2 satisfying T1 > T2 + T3 or T1 = Thd(T2 + T3) are found, the path from point A to point B can be planned based on the displacement function d(t) and velocity function v(t). The interpolation period in the path is flexibly adjusted according to the actual hardware communication bus protocol, generally 1ms-4ms per interpolation period; each interpolation value ΔS k It is calculated based on the displacement function d(t), that is, by substituting different times t from T1 to T7 into the displacement function d(t), the corresponding interpolation value ΔS can be obtained. k .

[0056] While the disclosure is as stated above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of this disclosure, and all such changes and modifications will fall within the protection scope of this invention.

Claims

1. A method for planning gantry-like trajectories of an articulated, decoupled robot, characterized in that, Includes the following steps: S1. Decompose the gantry-type motion trajectory from the starting point to the ending point into a horizontal trajectory and a vertical trajectory; S2. Decouple the robot's motion joints according to the horizontal and vertical trajectories to obtain the horizontal motion joints corresponding to the horizontal trajectory and the vertical motion joints corresponding to the vertical trajectory. S3, calculate the distance the horizontal joint needs to move; S1, the distance the vertical joint needs to lift; S2, the distance the vertical joint needs to lower; S3, and the angles the horizontal joint's axes need to rotate. , i represents the i-th axis of rotation on the horizontal motion joint; the modulus L that each axis of rotation on the horizontal motion joint needs to rotate is: n is the total number of rotation axes on the horizontally moving joint; S4. Calculate the horizontal movement time T1 of the joint at distance S1 based on the preset velocity v1, acceleration a1, and jerk j1. Calculate the vertical movement time T2 at distance S2 and the vertical movement time T3 at distance S3 based on the preset velocity v2, acceleration a2, and jerk j2. S5. Determine the relationship between T1, T2, and T3. If T1 > T2 + T3, proceed to step S9. If T1 ≤ T2 + T3, proceed to step S6. S6, within the range of speed v2 [ , If a new velocity v2 is found such that T1 > T2 + T3, proceed to step S9. If the range of the velocity v2 is within […], then… , If all values ​​in the range make T1≤T2+T3, then proceed to step S7; S7. Fix the range of the speed v2. , The final velocity v2 found within the range of acceleration a2 remains unchanged. , And the range of acceleration j2 [] , Within the range of [ ], find new accelerations a2 and j2 such that T1 > T2 + T3. If new accelerations a2 and j2 are found such that T1 > T2 + T3, proceed to step S9. If the range of acceleration a2 is [ , And the range of acceleration j2 [] , If all values ​​in the range make T1≤T2+T3, then proceed to step S8; S8. Fix the interval range of v2 respectively. , ], the range of acceleration a2 [ , And the range of acceleration j2 [] , The final velocity v2, acceleration a2, and jerk j2 found within the range [0, v1] remain unchanged; a new velocity v1 is found in the range [0, v1] such that T1 = Thd(T2 + T3), where Thd is the compensation value to ensure synchronization of the horizontal and vertical joints, and Thd >

1. S9. Based on the velocity v1, acceleration a1, jerk j1, velocity v2, acceleration a2, and jerk j2 satisfying T1>T2+T3 or T1=Thd(T2+T3), plan a path and obtain the interpolation value for each interpolation cycle in the path. , k represents the kth interpolation period; S10, Based on the interpolation values ​​described above The mapping functions for each axis on the horizontal and vertical motion joints are obtained. : And according to each of the interpolation values The path is interpolated according to each interpolation cycle.

2. The method for planning gantry-like trajectories of an articulated decoupled robot according to claim 1, characterized in that, In steps S6, S7, and S8, the new velocity v2, acceleration a2, jerk j2, and velocity v1 are all found using the binary search method.

3. A method for planning gantry-like trajectories of an articulated decoupled robot according to claim 1 or 2, characterized in that, In step S9, the path is a seven-segment S-shaped curve, wherein the displacement function for: ； ； Where v0 represents the initial velocity, j represents the jerk, a represents the acceleration, and v represents the maximum velocity. to Let t represent the time of each motion segment. to is any movement time within the total time of motion, and s represents any length within the total length of the curve; velocity function for: 。

Images